From e9a9922ae9be25c78ef49b183e8e3bb7451ee93f Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Mon, 8 Apr 2024 08:29:45 +0200 Subject: [PATCH 01/17] Mute warnings due to librealsense 3rdparty Fix build issues when x11 unavailable --- tutorial/segmentation/color/CMakeLists.txt | 25 +++++++++++++++++++ .../color/tutorial-hsv-range-tuner.cpp | 2 +- .../tutorial-hsv-segmentation-pcl-viewer.cpp | 2 +- .../color/tutorial-hsv-segmentation-pcl.cpp | 2 +- .../color/tutorial-hsv-segmentation.cpp | 8 +++--- 5 files changed, 33 insertions(+), 6 deletions(-) diff --git a/tutorial/segmentation/color/CMakeLists.txt b/tutorial/segmentation/color/CMakeLists.txt index 2d8f4b04d4..bafff92375 100644 --- a/tutorial/segmentation/color/CMakeLists.txt +++ b/tutorial/segmentation/color/CMakeLists.txt @@ -26,3 +26,28 @@ endforeach() foreach(data ${tutorial_data}) visp_copy_data(tutorial-hsv-segmentation-basic.cpp ${data}) endforeach() + +if(VISP_HAVE_REALSENSE2) + # Add specific build flag to turn off warnings coming from librealsense 3rd party + list(APPEND CXX_FLAGS_MUTE_WARNINGS_RS "/wd4244") + list(APPEND CXX_FLAGS_MUTE_WARNINGS_RS "/wd4267") + + list(APPEND CXX_FLAGS_MUTE_WARNINGS_RS "-Wno-deprecated-copy") + list(APPEND CXX_FLAGS_MUTE_WARNINGS_RS "-Wno-deprecated-declarations") + list(APPEND CXX_FLAGS_MUTE_WARNINGS_RS "-Wno-ignored-qualifiers") + list(APPEND CXX_FLAGS_MUTE_WARNINGS_RS "-Wno-overloaded-virtual") + list(APPEND CXX_FLAGS_MUTE_WARNINGS_RS "-Wno-pessimizing-move") + list(APPEND CXX_FLAGS_MUTE_WARNINGS_RS "-Wno-reorder") + list(APPEND CXX_FLAGS_MUTE_WARNINGS_RS "-Wno-sign-compare") + list(APPEND CXX_FLAGS_MUTE_WARNINGS_RS "-Wno-unused-function") + list(APPEND CXX_FLAGS_MUTE_WARNINGS_RS "-Wno-unused-parameter") + list(APPEND CXX_FLAGS_MUTE_WARNINGS_RS "-Wno-unqualified-std-cast-call") +endif() + +if(CXX_FLAGS_MUTE_WARNINGS_RS) + # Add specific build flag to turn off warnings + visp_set_source_file_compile_flag(tutorial-hsv-range-tuner.cpp ${CXX_FLAGS_MUTE_WARNINGS_RS}) + visp_set_source_file_compile_flag(tutorial-hsv-segmentation.cpp ${CXX_FLAGS_MUTE_WARNINGS_RS}) + visp_set_source_file_compile_flag(tutorial-hsv-segmentation-pcl.cpp ${CXX_FLAGS_MUTE_WARNINGS_RS}) + visp_set_source_file_compile_flag(tutorial-hsv-segmentation-pcl-viewer.cpp ${CXX_FLAGS_MUTE_WARNINGS_RS}) +endif() diff --git a/tutorial/segmentation/color/tutorial-hsv-range-tuner.cpp b/tutorial/segmentation/color/tutorial-hsv-range-tuner.cpp index 6577fa8465..5bed5a90d7 100644 --- a/tutorial/segmentation/color/tutorial-hsv-range-tuner.cpp +++ b/tutorial/segmentation/color/tutorial-hsv-range-tuner.cpp @@ -75,7 +75,7 @@ static void on_high_V_thresh_trackbar(int, void *) set_trackbar_V_max(hsv_values_trackbar[5]); } -int main(int argc, char *argv[]) +int main(int argc, const char *argv[]) { bool opt_save_img = false; std::string opt_hsv_filename = "calib/hsv-thresholds.yml"; diff --git a/tutorial/segmentation/color/tutorial-hsv-segmentation-pcl-viewer.cpp b/tutorial/segmentation/color/tutorial-hsv-segmentation-pcl-viewer.cpp index a7ceae34e4..6ca4686235 100644 --- a/tutorial/segmentation/color/tutorial-hsv-segmentation-pcl-viewer.cpp +++ b/tutorial/segmentation/color/tutorial-hsv-segmentation-pcl-viewer.cpp @@ -15,7 +15,7 @@ //! [Include vpDisplayPCL header] #include -int main(int argc, char **argv) +int main(int argc, const char *argv[]) { std::string opt_hsv_filename = "calib/hsv-thresholds.yml"; bool opt_pcl_textured = false; diff --git a/tutorial/segmentation/color/tutorial-hsv-segmentation-pcl.cpp b/tutorial/segmentation/color/tutorial-hsv-segmentation-pcl.cpp index 4742cc148d..b6961ad488 100644 --- a/tutorial/segmentation/color/tutorial-hsv-segmentation-pcl.cpp +++ b/tutorial/segmentation/color/tutorial-hsv-segmentation-pcl.cpp @@ -12,7 +12,7 @@ #include #include -int main(int argc, char **argv) +int main(int argc, const char *argv[]) { std::string opt_hsv_filename = "calib/hsv-thresholds.yml"; diff --git a/tutorial/segmentation/color/tutorial-hsv-segmentation.cpp b/tutorial/segmentation/color/tutorial-hsv-segmentation.cpp index b9f0b6002d..b2d2d9dd01 100644 --- a/tutorial/segmentation/color/tutorial-hsv-segmentation.cpp +++ b/tutorial/segmentation/color/tutorial-hsv-segmentation.cpp @@ -12,7 +12,7 @@ #include #include -int main(int argc, char **argv) +int main(int argc, const char *argv[]) { #if defined(VISP_HAVE_X11) std::string opt_hsv_filename = "calib/hsv-thresholds.yml"; @@ -90,7 +90,8 @@ int main(int argc, char **argv) } vpImage I; - int width, height; + int width = 848; + int height = 480; vpVideoReader g; #if defined(VISP_HAVE_REALSENSE2) @@ -99,7 +100,6 @@ int main(int argc, char **argv) if (use_realsense) { #if defined(VISP_HAVE_REALSENSE2) - width = 848; height = 480; int fps = 60; rs2::config config; config.enable_stream(RS2_STREAM_COLOR, width, height, RS2_FORMAT_RGBA8, fps); @@ -179,6 +179,8 @@ int main(int argc, char **argv) std::cout << "Mean loop time: " << total_loop_time / nb_iter << std::endl; #else + (void)argc; + (void)argv; std::cout << "This tutorial needs X11 3rdparty that is not enabled" << std::endl; #endif return EXIT_SUCCESS; From d619337f27b36eec753dd1b66e740c73e944cefe Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Mon, 8 Apr 2024 08:31:00 +0200 Subject: [PATCH 02/17] Satisfy sonarqube misra c++ quality rules --- modules/core/src/math/matrix/vpMatrix.cpp | 2 +- modules/core/src/math/misc/vpMath.cpp | 74 ++++++++----- modules/tracker/me/src/moving-edges/vpMe.cpp | 8 +- .../src/pose-estimation/vpPoseRansac.cpp | 103 +++++++++++------- 4 files changed, 112 insertions(+), 75 deletions(-) diff --git a/modules/core/src/math/matrix/vpMatrix.cpp b/modules/core/src/math/matrix/vpMatrix.cpp index 4463b6eda9..b4d43403b6 100644 --- a/modules/core/src/math/matrix/vpMatrix.cpp +++ b/modules/core/src/math/matrix/vpMatrix.cpp @@ -113,7 +113,7 @@ unsigned int vpMatrix::m_lapack_min_size = vpMatrix::m_lapack_min_size_default; #endif // Prototypes of specific functions -vpMatrix subblock(const vpMatrix &, unsigned int, unsigned int); +vpMatrix subblock(const vpMatrix &M, unsigned int col, unsigned int row); void compute_pseudo_inverse(const vpMatrix &U, const vpColVector &sv, const vpMatrix &V, unsigned int nrows, unsigned int ncols, double svThreshold, vpMatrix &Ap, int &rank_out, int *rank_in, diff --git a/modules/core/src/math/misc/vpMath.cpp b/modules/core/src/math/misc/vpMath.cpp index 661039251c..c7f9b1e2c7 100644 --- a/modules/core/src/math/misc/vpMath.cpp +++ b/modules/core/src/math/misc/vpMath.cpp @@ -214,7 +214,8 @@ bool vpMath::isFinite(float value) */ bool vpMath::isNumber(const std::string &str) { - for (size_t i = 0; i < str.size(); i++) { + size_t str_size = str.size(); + for (size_t i = 0; i < str_size; i++) { if (isdigit(str[i]) == false) { return false; } @@ -232,10 +233,12 @@ bool vpMath::isNumber(const std::string &str) */ double vpMath::mcosc(double cosx, double x) { - if (fabs(x) < ang_min_mc) + if (fabs(x) < ang_min_mc) { return 0.5; - else + } + else { return ((1.0 - cosx) / x / x); + } } /*! @@ -248,10 +251,12 @@ double vpMath::mcosc(double cosx, double x) */ double vpMath::msinc(double sinx, double x) { - if (fabs(x) < ang_min_mc) + if (fabs(x) < ang_min_mc) { return (1. / 6.0); - else - return ((1.0 - sinx / x) / x / x); + } + else { + return ((1.0 - (sinx / x)) / x / x); + } } /*! @@ -263,10 +268,12 @@ double vpMath::msinc(double sinx, double x) */ double vpMath::sinc(double x) { - if (fabs(x) < ang_min_sinc) + if (fabs(x) < ang_min_sinc) { return 1.0; - else + } + else { return sin(x) / x; + } } /*! Compute sinus cardinal \f$ \frac{sin(x)}{x}\f$. @@ -278,10 +285,12 @@ double vpMath::sinc(double x) */ double vpMath::sinc(double sinx, double x) { - if (fabs(x) < ang_min_sinc) + if (fabs(x) < ang_min_sinc) { return 1.0; - else + } + else { return (sinx / x); + } } /*! @@ -301,7 +310,7 @@ double vpMath::getMean(const std::vector &v) double sum = std::accumulate(v.begin(), v.end(), 0.0); - return sum / (double)size; + return sum / (static_cast(size)); } /*! @@ -324,11 +333,11 @@ double vpMath::getMedian(const std::vector &v) std::nth_element(v_copy.begin(), v_copy.begin() + n, v_copy.end()); double val_n = v_copy[n]; - if (size % 2 == 1) { + if ((size % 2) == 1) { return val_n; } else { - std::nth_element(v_copy.begin(), v_copy.begin() + n - 1, v_copy.end()); + std::nth_element(v_copy.begin(), v_copy.begin() + (n - 1), v_copy.end()); return 0.5 * (val_n + v_copy[n - 1]); } } @@ -358,8 +367,8 @@ double vpMath::getStdev(const std::vector &v, bool useBesselCorrection) #endif double sq_sum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0); - double divisor = (double)v.size(); - if (useBesselCorrection && v.size() > 1) { + double divisor = static_cast (v.size()); + if (useBesselCorrection && (v.size() > 1)) { divisor = divisor - 1; } @@ -386,7 +395,8 @@ double vpMath::lineFitting(const std::vector &imPts, double &a, do } double x_mean = 0, y_mean = 0; - for (size_t i = 0; i < imPts.size(); i++) { + size_t imPts_size = imPts.size(); + for (size_t i = 0; i < imPts_size; i++) { const vpImagePoint &imPt = imPts[i]; x_mean += imPt.get_u(); y_mean += imPt.get_v(); @@ -395,7 +405,8 @@ double vpMath::lineFitting(const std::vector &imPts, double &a, do y_mean /= imPts.size(); vpMatrix AtA(2, 2, 0.0); - for (size_t i = 0; i < imPts.size(); i++) { + imPts_size = imPts.size(); + for (size_t i = 0; i < imPts_size; i++) { const vpImagePoint &imPt = imPts[i]; AtA[0][0] += (imPt.get_u() - x_mean) * (imPt.get_u() - x_mean); AtA[0][1] += (imPt.get_u() - x_mean) * (imPt.get_v() - y_mean); @@ -409,14 +420,15 @@ double vpMath::lineFitting(const std::vector &imPts, double &a, do a = eigenvectors[0][0]; b = eigenvectors[1][0]; - c = a * x_mean + b * y_mean; + c = (a * x_mean) + (b * y_mean); double error = 0; - for (size_t i = 0; i < imPts.size(); i++) { + imPts_size = imPts.size(); + for (size_t i = 0; i < imPts_size; i++) { double x0 = imPts[i].get_u(); double y0 = imPts[i].get_v(); - error += std::fabs(a * x0 + b * y0 - c); + error += std::fabs((a * x0) + ((b * y0) - c)); } return error / imPts.size(); @@ -573,9 +585,9 @@ std::vector > vpMath::computeRegularPointsOnSphere(uns { assert(maxPoints > 0); - double a = 4.0 * M_PI / maxPoints; + double a = (4.0 * M_PI) / maxPoints; double d = sqrt(a); - int m_theta = int(round(M_PI / d)); + int m_theta = static_cast(round(M_PI / d)); double d_theta = M_PI / m_theta; double d_phi = a / d_theta; @@ -587,11 +599,11 @@ std::vector > vpMath::computeRegularPointsOnSphere(uns #endif for (int m = 0; m < m_theta; m++) { - double theta = M_PI * (m + 0.5) / m_theta; - int m_phi = static_cast(round(2.0 * M_PI * sin(theta) / d_phi)); + double theta = (M_PI * (m + 0.5)) / m_theta; + int m_phi = static_cast(round((2.0 * M_PI * sin(theta)) / d_phi)); for (int n = 0; n < m_phi; n++) { - double phi = 2.0 * M_PI * n / m_phi; + double phi = (2.0 * M_PI * n) / m_phi; double lon = phi; double lat = M_PI_2 - theta; lonlat_vec.push_back(std::make_pair(deg(lon), deg(lat))); @@ -625,7 +637,8 @@ std::vector vpMath::getLocalTangentPlaneTransformations(con { std::vector ecef_M_local_vec; ecef_M_local_vec.reserve(lonlatVec.size()); - for (size_t i = 0; i < lonlatVec.size(); i++) { + size_t lonlatVec_size = lonlatVec.size(); + for (size_t i = 0; i < lonlatVec_size; i++) { double lonDeg = lonlatVec[i].first; double latDeg = lonlatVec[i].second; @@ -693,7 +706,8 @@ vpColVector vpMath::deg(const vpRotationVector &r) throw(vpException(vpException::fatalError, "Cannot convert angles of a quaternion vector in degrees!")); } vpColVector r_deg(r.size()); - for (unsigned int i = 0; i < r.size(); i++) { + unsigned int r_size = r.size(); + for (unsigned int i = 0; i < r_size; i++) { r_deg[i] = vpMath::deg(r[i]); } return r_deg; @@ -708,7 +722,8 @@ vpColVector vpMath::deg(const vpRotationVector &r) vpColVector vpMath::deg(const vpColVector &r) { vpColVector r_deg(r.size()); - for (unsigned int i = 0; i < r.size(); i++) { + unsigned int r_size = r.size(); + for (unsigned int i = 0; i < r_size; i++) { r_deg[i] = vpMath::deg(r[i]); } return r_deg; @@ -723,7 +738,8 @@ vpColVector vpMath::deg(const vpColVector &r) vpColVector vpMath::rad(const vpColVector &r) { vpColVector r_rad(r.size()); - for (unsigned int i = 0; i < r.size(); i++) { + unsigned int r_size = r.size(); + for (unsigned int i = 0; i < r_size; i++) { r_rad[i] = vpMath::rad(r[i]); } return r_rad; diff --git a/modules/tracker/me/src/moving-edges/vpMe.cpp b/modules/tracker/me/src/moving-edges/vpMe.cpp index 016a645e21..990293c697 100644 --- a/modules/tracker/me/src/moving-edges/vpMe.cpp +++ b/modules/tracker/me/src/moving-edges/vpMe.cpp @@ -234,7 +234,7 @@ static double surfaceRelative(vpPoint2Dt P, vpPoint2Dt Q, double Xmin, double Ym (vpMath::maximum(std::fabs(P.x), std::fabs(Xmin)) * std::numeric_limits::epsilon())) && (std::fabs(Q.x - Xmax) <= (vpMath::maximum(std::fabs(Q.x), std::fabs(Xmax)) * std::numeric_limits::epsilon()))) { - return fabs(Ymax + Ymin - P.y - Q.y); + return fabs((Ymax + Ymin) - (P.y - Q.y)); } // Case (P.y=Ymin and Q.y==Ymax) or (Q.y=Ymin and P.y==Ymax) @@ -246,7 +246,7 @@ static double surfaceRelative(vpPoint2Dt P, vpPoint2Dt Q, double Xmin, double Ym (vpMath::maximum(std::fabs(Q.y), std::fabs(Ymin)) * std::numeric_limits::epsilon())) && (std::fabs(P.y - Ymax) <= (vpMath::maximum(std::fabs(P.y), std::fabs(Ymax)) * std::numeric_limits::epsilon())))) { - return fabs(Xmax + Xmin - P.x - Q.x); + return fabs((Xmax + Xmin) - (P.x - Q.x)); } // Case P.x=Xmin and Q.y=Ymax @@ -329,8 +329,8 @@ static void calculMasques(vpColVector &angle, // definitions des angles theta // ====================== M[i_theta].resize(n, n); // allocation (si necessaire) - for (i = 0, Y = -moitie + 0.5; i < n; ++i, ++Y) { - for (j = 0, X = -moitie + 0.5; j < n; ++j, ++X) { + for (i = 0, Y = (-moitie + 0.5); i < n; ++i, ++Y) { + for (j = 0, X = (-moitie + 0.5); j < n; ++j, ++X) { // produit vectoriel dir_droite*(X,Y) int sgn = vpMath::sign((cos_theta * Y) - (sin_theta * X)); diff --git a/modules/vision/src/pose-estimation/vpPoseRansac.cpp b/modules/vision/src/pose-estimation/vpPoseRansac.cpp index e476fb83be..b6b66b9537 100644 --- a/modules/vision/src/pose-estimation/vpPoseRansac.cpp +++ b/modules/vision/src/pose-estimation/vpPoseRansac.cpp @@ -53,7 +53,7 @@ #include #endif -#define eps 1e-6 +#define EPS 1e-6 namespace { @@ -62,31 +62,41 @@ struct CompareObjectPointDegenerate { bool operator()(const vpPoint &point1, const vpPoint &point2) const { + bool rc = false; const double dist1 = - point1.get_oX() * point1.get_oX() + point1.get_oY() * point1.get_oY() + point1.get_oZ() * point1.get_oZ(); + (point1.get_oX() * point1.get_oX()) + (point1.get_oY() * point1.get_oY()) + (point1.get_oZ() * point1.get_oZ()); const double dist2 = - point2.get_oX() * point2.get_oX() + point2.get_oY() * point2.get_oY() + point2.get_oZ() * point2.get_oZ(); - if (dist1 - dist2 < -3 * eps * eps) + (point2.get_oX() * point2.get_oX()) + (point2.get_oY() * point2.get_oY()) + (point2.get_oZ() * point2.get_oZ()); + + if ((dist1 - dist2) < (-3 * EPS * EPS)) { return true; - if (dist1 - dist2 > 3 * eps * eps) + } + if ((dist1 - dist2) > (3 * EPS * EPS)) { return false; + } - if (point1.oP[0] - point2.oP[0] < -eps) + if ((point1.oP[0] - point2.oP[0]) < -EPS) { return true; - if (point1.oP[0] - point2.oP[0] > eps) + } + if ((point1.oP[0] - point2.oP[0]) > EPS) { return false; + } - if (point1.oP[1] - point2.oP[1] < -eps) + if ((point1.oP[1] - point2.oP[1]) < -EPS) { return true; - if (point1.oP[1] - point2.oP[1] > eps) + } + if ((point1.oP[1] - point2.oP[1]) > EPS) { return false; + } - if (point1.oP[2] - point2.oP[2] < -eps) + if ((point1.oP[2] - point2.oP[2]) < -EPS) { return true; - if (point1.oP[2] - point2.oP[2] > eps) + } + if ((point1.oP[2] - point2.oP[2]) > EPS) { return false; + } - return false; + return rc; } }; @@ -95,24 +105,31 @@ struct CompareImagePointDegenerate { bool operator()(const vpPoint &point1, const vpPoint &point2) const { - const double dist1 = point1.get_x() * point1.get_x() + point1.get_y() * point1.get_y(); - const double dist2 = point2.get_x() * point2.get_x() + point2.get_y() * point2.get_y(); - if (dist1 - dist2 < -2 * eps * eps) + bool rc = false; + const double dist1 = (point1.get_x() * point1.get_x()) + (point1.get_y() * point1.get_y()); + const double dist2 = (point2.get_x() * point2.get_x()) + (point2.get_y() * point2.get_y()); + if ((dist1 - dist2) < (-2 * EPS * EPS)) { return true; - if (dist1 - dist2 > 2 * eps * eps) + } + if ((dist1 - dist2) > (2 * EPS * EPS)) { return false; + } - if (point1.p[0] - point2.p[0] < -eps) + if ((point1.p[0] - point2.p[0]) < -EPS) { return true; - if (point1.p[0] - point2.p[0] > eps) + } + if ((point1.p[0] - point2.p[0]) > EPS) { return false; + } - if (point1.p[1] - point2.p[1] < -eps) + if ((point1.p[1] - point2.p[1]) < -EPS) { return true; - if (point1.p[1] - point2.p[1] > eps) + } + if ((point1.p[1] - point2.p[1]) > EPS) { return false; + } - return false; + return rc; } }; @@ -123,9 +140,9 @@ 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)); + 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))); } vpPoint m_pt; @@ -134,14 +151,14 @@ struct FindDegeneratePoint bool vpPose::vpRansacFunctor::poseRansacImpl() { - const unsigned int size = (unsigned int)m_listOfUniquePoints.size(); + const unsigned int size = static_cast(m_listOfUniquePoints.size()); unsigned int nbMinRandom = 4; int nbTrials = 0; vpPoint p; // Point used to project using the estimated pose bool foundSolution = false; - while (nbTrials < m_ransacMaxTrials && m_nbInliers < m_ransacNbInlierConsensus) { + while ((nbTrials < m_ransacMaxTrials) && (m_nbInliers < m_ransacNbInlierConsensus)) { // Hold the list of the index of the inliers (points in the consensus set) std::vector cur_consensus; // Hold the list of the index of the outliers @@ -209,6 +226,7 @@ bool vpPose::vpRansacFunctor::poseRansacImpl() r_min = poseMin.computeResidual(cMo_tmp); } catch (...) { + // no need to take action } // If residual returned is not a number (NAN), set valid to false @@ -218,7 +236,7 @@ bool vpPose::vpRansacFunctor::poseRansacImpl() // If at pose computation is OK we can continue, otherwise pick another random set if (is_pose_valid) { - double r = sqrt(r_min) / (double)nbMinRandom; // FS should be r = sqrt(r_min / (double)nbMinRandom); + double r = sqrt(r_min) / static_cast(nbMinRandom); // FS should be r = sqrt(r_min / (double)nbMinRandom); // Filter the pose using some criterion (orientation angles, // translations, etc.) bool isPoseValid = true; @@ -233,7 +251,7 @@ bool vpPose::vpRansacFunctor::poseRansacImpl() m_cMo = cMo_tmp; } - if (isPoseValid && r < m_ransacThreshold) { + 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(); @@ -440,22 +458,22 @@ bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneo if (foundSolution) { unsigned int nbMinRandom = 4; - // std::cout << "Nombre d'inliers " << nbInliers << std::endl ; + // std::cout << "Nombre d'inliers " << nbInliers << std::endl // Display the random picked points /* - std::cout << "Randoms : "; + std::cout << "Randoms : " for(unsigned int i = 0 ; i < cur_randoms.size() ; i++) - std::cout << cur_randoms[i] << " "; - std::cout << std::endl; + std::cout << cur_randoms[i] << " " + std::cout << std::endl */ // Display the outliers /* - std::cout << "Outliers : "; + std::cout << "Outliers : " for(unsigned int i = 0 ; i < cur_outliers.size() ; i++) - std::cout << cur_outliers[i] << " "; - std::cout << std::endl; + std::cout << cur_outliers[i] << " " + std::cout << std::endl */ // Even if the cardinality of the best consensus set is inferior to @@ -468,7 +486,8 @@ bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneo // Refine the solution using all the points in the consensus set and // with VVS pose estimation vpPose pose; - for (size_t i = 0; i < best_consensus.size(); i++) { + size_t best_consensus_size = best_consensus.size(); + for (size_t i = 0; i < best_consensus_size; i++) { vpPoint pt = listOfUniquePoints[best_consensus[i]]; pose.addPoint(pt); @@ -478,7 +497,7 @@ bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneo // Update the list of inlier index for (std::vector::const_iterator it_index = best_consensus.begin(); it_index != best_consensus.end(); ++it_index) { - ransacInlierIndex.push_back((unsigned int)mapOfUniquePointIndex[*it_index]); + ransacInlierIndex.push_back(static_cast(mapOfUniquePointIndex[*it_index])); } pose.setCovarianceComputation(computeCovariance); @@ -487,7 +506,7 @@ bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneo // In some rare cases, the final pose could not respect the pose // criterion even if the 4 minimal points picked respect the pose // criterion. - if (func != nullptr && !func(cMo)) { + if ((func != nullptr) && (!func(cMo))) { return false; } @@ -535,7 +554,7 @@ int vpPose::computeRansacIterations(double probability, double epsilon, const in N = log(std::max(1.0 - probability, std::numeric_limits::epsilon())) / logval; if (logval < 0.0 && N < maxIterations) { - return (int)ceil(N); + return static_cast(ceil(N)); } return maxIterations; @@ -549,8 +568,10 @@ void vpPose::findMatch(std::vector &p2D, std::vector &p3D, { vpPose pose; - for (unsigned int i = 0; i < p2D.size(); i++) { - for (unsigned int j = 0; j < p3D.size(); j++) { + unsigned int p2D_size = p2D.size(); + unsigned int p3D_size = p3D.size(); + for (unsigned int i = 0; i < p2D_size; i++) { + for (unsigned int j = 0; j < p3D_size; j++) { vpPoint pt(p3D[j].getWorldCoordinates()); pt.set_x(p2D[i].get_x()); pt.set_y(p2D[i].get_y()); From fc679ff99feb7377a1722eaf7edc18bb9ede0041 Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Mon, 8 Apr 2024 10:00:42 +0200 Subject: [PATCH 03/17] Satisfy quality rules --- modules/core/src/camera/vpXmlParserCamera.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/core/src/camera/vpXmlParserCamera.cpp b/modules/core/src/camera/vpXmlParserCamera.cpp index 4af8c03b72..3c30c60978 100644 --- a/modules/core/src/camera/vpXmlParserCamera.cpp +++ b/modules/core/src/camera/vpXmlParserCamera.cpp @@ -379,7 +379,7 @@ class vpXmlParserCamera::Impl double kdu = cam_tmp.get_kdu(); std::vector distortion_coeffs; vpXmlCodeSequenceType back = SEQUENCE_OK; - int validation = 0; + unsigned int validation = 0; for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) { // vpDEBUG_TRACE (15, "Carac : %s.", node ->name); From dcf067b0415f84cb801caa2d66c83812b7271ace Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Mon, 8 Apr 2024 10:01:49 +0200 Subject: [PATCH 04/17] Satisfy quality rules by have files with less than 3000 lines --- modules/core/src/image/vpImageConvert.cpp | 2594 ----------------- .../testColorConversion.cpp | 103 +- 2 files changed, 93 insertions(+), 2604 deletions(-) diff --git a/modules/core/src/image/vpImageConvert.cpp b/modules/core/src/image/vpImageConvert.cpp index b4a079bbb1..83db7d22ff 100644 --- a/modules/core/src/image/vpImageConvert.cpp +++ b/modules/core/src/image/vpImageConvert.cpp @@ -1044,2600 +1044,6 @@ void vpImageConvert::convert(const yarp::sig::ImageOf *src, #endif -#define vpSAT(c) \ - if (c & (~255)) { \ - if (c < 0) \ - {c = 0;} \ - else \ - {c = 255;} \ - } -/*! - Convert an image from YUYV 4:2:2 (y0 u01 y1 v01 y2 u23 y3 v23 ...) to RGB32. - Destination rgba memory area has to be allocated before. - - The alpha component of the converted image is set to vpRGBa::alpha_default. - - \param[in] yuyv : Pointer to the bitmap containing the YUYV 4:2:2 data. - \param[out] rgba : Pointer to the RGB32 bitmap that should be allocated with a size of \e width * \e height * 4. - \param[in] width, height : Image size. - - \sa YUV422ToRGBa() -*/ -void vpImageConvert::YUYVToRGBa(unsigned char *yuyv, unsigned char *rgba, unsigned int width, unsigned int height) -{ - unsigned char *s; - unsigned char *d; - int w, h; - int r, g, b, cr, cg, cb, y1, y2; - - h = static_cast(height); - w = static_cast(width); - s = yuyv; - d = rgba; - while (h--) { - int c = w >> 1; - while (c--) { - y1 = *s++; - cb = ((*s - 128) * 454) >> 8; - cg = (*s++ - 128) * 88; - y2 = *s++; - cr = ((*s - 128) * 359) >> 8; - cg = (cg + ((*s++ - 128) * 183)) >> 8; - - r = y1 + cr; - b = y1 + cb; - g = y1 - cg; - vpSAT(r) vpSAT(g) vpSAT(b) - - *d++ = static_cast(r); - *d++ = static_cast(g); - *d++ = static_cast(b); - *d++ = vpRGBa::alpha_default; - - r = y2 + cr; - b = y2 + cb; - g = y2 - cg; - vpSAT(r) vpSAT(g) vpSAT(b) - - *d++ = static_cast(r); - *d++ = static_cast(g); - *d++ = static_cast(b); - *d++ = vpRGBa::alpha_default; - } - } -} - -/*! - Convert an image from YUYV 4:2:2 (y0 u01 y1 v01 y2 u23 y3 v23 ...) - to RGB24. Destination rgb memory area has to be allocated before. - - \param[in] yuyv : Pointer to the bitmap containing the YUYV 4:2:2 data. - \param[out] rgb : Pointer to the RGB32 bitmap that should be allocated with a size of \e width * \e height * 3. - \param[in] width, height : Image size. - - \sa YUV422ToRGB() -*/ -void vpImageConvert::YUYVToRGB(unsigned char *yuyv, unsigned char *rgb, unsigned int width, unsigned int height) -{ - unsigned char *s; - unsigned char *d; - int h, w; - int r, g, b, cr, cg, cb, y1, y2; - - h = static_cast(height); - w = static_cast(width); - s = yuyv; - d = rgb; - while (h--) { - int c = w >> 1; - while (c--) { - y1 = *s++; - cb = ((*s - 128) * 454) >> 8; - cg = (*s++ - 128) * 88; - y2 = *s++; - cr = ((*s - 128) * 359) >> 8; - cg = (cg + ((*s++ - 128) * 183)) >> 8; - - r = y1 + cr; - b = y1 + cb; - g = y1 - cg; - vpSAT(r) vpSAT(g) vpSAT(b) - - *d++ = static_cast(r); - *d++ = static_cast(g); - *d++ = static_cast(b); - - r = y2 + cr; - b = y2 + cb; - g = y2 - cg; - vpSAT(r) vpSAT(g) vpSAT(b) - - *d++ = static_cast(r); - *d++ = static_cast(g); - *d++ = static_cast(b); - } - } -} - -/*! - Convert an image from YUYV 4:2:2 (y0 u01 y1 v01 y2 u23 y3 v23 ...) - to grey. Destination grey memory area has to be allocated before. - - \param[in] yuyv : Pointer to the bitmap containing the YUYV 4:2:2 data. - \param[out] grey : Pointer to the 8-bits grey bitmap that should be allocated with a size of width * height. - \param[in] size : Image size corresponding to width * height. - - \sa YUV422ToGrey() -*/ -void vpImageConvert::YUYVToGrey(unsigned char *yuyv, unsigned char *grey, unsigned int size) -{ - unsigned int i = 0, j = 0; - const unsigned int doubleSize = size * 2; - while (j < doubleSize) { - grey[i++] = yuyv[j]; - grey[i++] = yuyv[j + 2]; - j += 4; - } -} - -/*! - Convert YUV 4:1:1 (u y1 y2 v y3 y4) images into RGBa images. The alpha - component of the converted image is set to vpRGBa::alpha_default. - - \param[in] yuv : Pointer to the bitmap containing the YUV 4:1:1 data. - \param[out] rgba : Pointer to the RGBA 32-bits bitmap that should be allocated with a size of width * height * 4. - \param[in] size : Image size corresponding to width * height. - -*/ -void vpImageConvert::YUV411ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int size) -{ -#if 1 - // std::cout << "call optimized ConvertYUV411ToRGBa()" << std::endl; - for (unsigned int i = size / 4; i; --i) { - int U = static_cast((*yuv++ - 128) * 0.354); - int U5 = 5 * U; - int Y0 = *yuv++; - int Y1 = *yuv++; - int V = static_cast((*yuv++ - 128) * 0.707); - int V2 = 2 * V; - int Y2 = *yuv++; - int Y3 = *yuv++; - int UV = -U - V; - - // Original equations - // R = Y + 1.402 V - // G = Y - 0.344 U - 0.714 V - // B = Y + 1.772 U - int R = Y0 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - int G = Y0 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - int B = Y0 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba++ = vpRGBa::alpha_default; - - //--- - R = Y1 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y1 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y1 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba++ = vpRGBa::alpha_default; - - //--- - R = Y2 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y2 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y2 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba++ = vpRGBa::alpha_default; - - //--- - R = Y3 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y3 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y3 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba++ = vpRGBa::alpha_default; - } -#else - // tres tres lent .... - unsigned int i = 0, j = 0; - unsigned char r, g, b; - const unsigned int iterLimit = (numpixels * 3) / 2; - while (j < iterLimit) { - - YUVToRGB(yuv[j + 1], yuv[j], yuv[j + 3], r, g, b); - rgba[i] = r; - rgba[i + 1] = g; - rgba[i + 2] = b; - rgba[i + 3] = vpRGBa::alpha_default; - i += 4; - - YUVToRGB(yuv[j + 2], yuv[j], yuv[j + 3], r, g, b); - rgba[i] = r; - rgba[i + 1] = g; - rgba[i + 2] = b; - rgba[i + 3] = vpRGBa::alpha_default; - i += 4; - - YUVToRGB(yuv[j + 4], yuv[j], yuv[j + 3], r, g, b); - rgba[i] = r; - rgba[i + 1] = g; - rgba[i + 2] = b; - rgba[i + 3] = vpRGBa::alpha_default; - i += 4; - - YUVToRGB(yuv[j + 5], yuv[j], yuv[j + 3], r, g, b); - rgba[i] = r; - rgba[i + 1] = g; - rgba[i + 2] = b; - rgba[i + 3] = vpRGBa::alpha_default; - i += 4; - - j += 6; - } -#endif -} - -/*! - Convert YUV 4:2:2 (u01 y0 v01 y1 u23 y2 v23 y3 ...) images into RGBa images. - Destination rgba memory area has to be allocated before. - - The alpha component of the converted image is set to vpRGBa::alpha_default. - - \param[in] yuv : Pointer to the bitmap containing the YUV 4:2:2 data. - \param[out] rgba : Pointer to the RGBA 32-bits bitmap that should be allocated with a size of width * height * 4. - \param[in] size : Image size corresponding to width * height. - - \sa YUYVToRGBa() -*/ -void vpImageConvert::YUV422ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int size) -{ - -#if 1 - // std::cout << "call optimized convertYUV422ToRGBa()" << std::endl; - for (unsigned int i = size / 2; i; --i) { - int U = static_cast((*yuv++ - 128) * 0.354); - int U5 = 5 * U; - int Y0 = *yuv++; - int V = static_cast((*yuv++ - 128) * 0.707); - int V2 = 2 * V; - int Y1 = *yuv++; - int UV = -U - V; - - //--- - int R = Y0 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - int G = Y0 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - int B = Y0 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba++ = vpRGBa::alpha_default; - - //--- - R = Y1 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y1 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y1 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba++ = vpRGBa::alpha_default; - } - -#else - // tres tres lent .... - unsigned int i = 0, j = 0; - unsigned char r, g, b; - const unsigned int doubleSize = size * 2; - while (j < doubleSize) { - - YUVToRGB(yuv[j + 1], yuv[j], yuv[j + 2], r, g, b); - rgba[i] = r; - rgba[i + 1] = g; - rgba[i + 2] = b; - rgba[i + 3] = vpRGBa::alpha_default; - i += 4; - - YUVToRGB(yuv[j + 3], yuv[j], yuv[j + 2], r, g, b); - rgba[i] = r; - rgba[i + 1] = g; - rgba[i + 2] = b; - rgba[i + 3] = vpRGBa::alpha_default; - i += 4; - j += 4; - } -#endif -} - -/*! - Convert YUV 4:1:1 (u y1 y2 v y3 y4) into a grey image. - - \param[in] yuv : Pointer to the bitmap containing the YUV 4:1:1 data. - \param[out] grey : Pointer to the 8-bits grey bitmap that should be allocated with a size of width * height. - \param[in] size : Image size corresponding to width * height. - -*/ -void vpImageConvert::YUV411ToGrey(unsigned char *yuv, unsigned char *grey, unsigned int size) -{ - unsigned int i = 0, j = 0; - const unsigned int iterLimit = (size * 3) / 2; - while (j < iterLimit) { - grey[i] = yuv[j + 1]; - grey[i + 1] = yuv[j + 2]; - grey[i + 2] = yuv[j + 4]; - grey[i + 3] = yuv[j + 5]; - - i += 4; - - j += 6; - } -} - -/*! - Convert YUV 4:2:2 (u01 y0 v01 y1 u23 y2 v23 y3 ...) images into RGB images. - Destination rgb memory area has to be allocated before. - - \param[in] yuv : Pointer to the bitmap containing the YUV 4:2:2 data. - \param[out] rgb : Pointer to the 24-bits RGB bitmap that should be allocated with a size of width * height * 3. - \param[in] size : Image size corresponding to width * height. - - \sa YUYVToRGB() -*/ -void vpImageConvert::YUV422ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int size) -{ -#if 1 - // std::cout << "call optimized convertYUV422ToRGB()" << std::endl; - for (unsigned int i = size / 2; i; --i) { - int U = static_cast((*yuv++ - 128) * 0.354); - int U5 = 5 * U; - int Y0 = *yuv++; - int V = static_cast((*yuv++ - 128) * 0.707); - int V2 = 2 * V; - int Y1 = *yuv++; - int UV = -U - V; - - //--- - int R = Y0 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - int G = Y0 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - int B = Y0 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); - - //--- - R = Y1 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y1 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y1 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); - } - -#else - // tres tres lent .... - unsigned int i = 0, j = 0; - unsigned char r, g, b; - const unsigned int doubleSize = size * 2; - while (j < doubleSize) { - - YUVToRGB(yuv[j + 1], yuv[j], yuv[j + 2], r, g, b); - rgb[i] = r; - rgb[i + 1] = g; - rgb[i + 2] = b; - i += 3; - - YUVToRGB(yuv[j + 3], yuv[j], yuv[j + 2], r, g, b); - rgb[i] = r; - rgb[i + 1] = g; - rgb[i + 2] = b; - i += 3; - j += 4; - } -#endif -} - -/*! - Convert YUV 4:2:2 (u01 y0 v01 y1 u23 y2 v23 y3 ...) images into a grey image. - Destination grey memory area has to be allocated before. - - \param[in] yuv : Pointer to the bitmap containing the YUV 4:2:2 data. - \param[out] grey : Pointer to the 8-bits grey bitmap that should be allocated with a size of width * height. - \param[in] size : Image size corresponding to width * height. - - \sa YUYVToGrey() -*/ -void vpImageConvert::YUV422ToGrey(unsigned char *yuv, unsigned char *grey, unsigned int size) -{ - unsigned int i = 0, j = 0; - const unsigned int doubleSize = size * 2; - - while (j < doubleSize) { - grey[i++] = yuv[j + 1]; - grey[i++] = yuv[j + 3]; - j += 4; - } -} - -/*! - Convert YUV 4:1:1 (u y1 y2 v y3 y4) into a RGB 24bits image. - - \param[in] yuv : Pointer to the bitmap containing the YUV 4:1:1 data. - \param[out] rgb : Pointer to the 24-bits RGB bitmap that should be allocated with a size of width * height * 3. - \param[in] size : Image size corresponding to width * height. - -*/ -void vpImageConvert::YUV411ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int size) -{ -#if 1 - // std::cout << "call optimized ConvertYUV411ToRGB()" << std::endl; - for (unsigned int i = size / 4; i; --i) { - int U = static_cast((*yuv++ - 128) * 0.354); - int U5 = 5 * U; - int Y0 = *yuv++; - int Y1 = *yuv++; - int V = static_cast((*yuv++ - 128) * 0.707); - int V2 = 2 * V; - int Y2 = *yuv++; - int Y3 = *yuv++; - int UV = -U - V; - - // Original equations - // R = Y + 1.402 V - // G = Y - 0.344 U - 0.714 V - // B = Y + 1.772 U - int R = Y0 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - int G = Y0 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - int B = Y0 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); - - //--- - R = Y1 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y1 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y1 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); - - //--- - R = Y2 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y2 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y2 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); - - //--- - R = Y3 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y3 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y3 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); - } -#else - // tres tres lent .... - - unsigned int i = 0, j = 0; - unsigned char r, g, b; - - const unsigned int iterLimit = (size * 3) / 2; - while (j < iterLimit) { - YUVToRGB(yuv[j + 1], yuv[j], yuv[j + 3], r, g, b); - rgb[i] = r; - rgb[i + 1] = g; - rgb[i + 2] = b; - i += 3; - - YUVToRGB(yuv[j + 2], yuv[j], yuv[j + 3], r, g, b); - rgb[i] = r; - rgb[i + 1] = g; - rgb[i + 2] = b; - i += 3; - - YUVToRGB(yuv[j + 4], yuv[j], yuv[j + 3], r, g, b); - rgb[i] = r; - rgb[i + 1] = g; - rgb[i + 2] = b; - i += 3; - - YUVToRGB(yuv[j + 5], yuv[j], yuv[j + 3], r, g, b); - rgb[i] = r; - rgb[i + 1] = g; - rgb[i + 2] = b; - i += 3; - // TRACE("r= %d g=%d b=%d", r, g, b); - - j += 6; - } -#endif -} - -/*! - Convert YUV 4:2:0 [Y(NxM), U(N/2xM/2), V(N/2xM/2)] image into a RGBa image. - - The alpha component of the converted image is set to vpRGBa::alpha_default. - - \param[in] yuv : Pointer to the bitmap containing the YUV 4:2:0 data. - \param[out] rgba : Pointer to the 32-bits RGBA bitmap that should be allocated with a size of width * height * 4. - \param[in] width, height : Image size. - -*/ -void vpImageConvert::YUV420ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int width, unsigned int height) -{ - // std::cout << "call optimized ConvertYUV420ToRGBa()" << std::endl; - int U, V, R, G, B, V2, U5, UV; - int Y0, Y1, Y2, Y3; - unsigned int size = width * height; - unsigned char *iU = yuv + size; - unsigned char *iV = yuv + ((5 * size) / 4); - const unsigned int halfHeight = height / 2, halfWidth = width / 2; - for (unsigned int i = 0; i < halfHeight; ++i) { - for (unsigned int j = 0; j < halfWidth; ++j) { - U = static_cast((*iU++ - 128) * 0.354); - U5 = 5 * U; - V = static_cast((*iV++ - 128) * 0.707); - V2 = 2 * V; - UV = -U - V; - Y0 = *yuv++; - Y1 = *yuv; - yuv = yuv + (width - 1); - Y2 = *yuv++; - Y3 = *yuv; - yuv = (yuv - width) + 1; - - // Original equations - // R = Y + 1.402 V - // G = Y - 0.344 U - 0.714 V - // B = Y + 1.772 U - R = Y0 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y0 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y0 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba++ = vpRGBa::alpha_default; - - //--- - R = Y1 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y1 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y1 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba = vpRGBa::alpha_default; - rgba = rgba + 4 * width - 7; - - //--- - R = Y2 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y2 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y2 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba++ = vpRGBa::alpha_default; - - //--- - R = Y3 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y3 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y3 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba = vpRGBa::alpha_default; - rgba = (rgba - (4 * width)) + 1; - } - yuv += width; - rgba += 4 * width; - } -} - -/*! - Convert YUV 4:2:0 [Y(NxM), U(N/2xM/2), V(N/2xM/2)] image into a RGB image. - - \param[in] yuv : Pointer to the bitmap containing the YUV 4:2:0 data. - \param[out] rgb : Pointer to the 24-bits RGB bitmap that should be allocated with a size of width * height * 3. - \param[in] width, height : Image size. - -*/ -void vpImageConvert::YUV420ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int width, unsigned int height) -{ - // std::cout << "call optimized ConvertYUV420ToRGB()" << std::endl; - int U, V, R, G, B, V2, U5, UV; - int Y0, Y1, Y2, Y3; - unsigned int size = width * height; - unsigned char *iU = yuv + size; - unsigned char *iV = yuv + ((5 * size) / 4); - const unsigned int halfHeight = height / 2, halfWidth = width / 2; - for (unsigned int i = 0; i < halfHeight; ++i) { - for (unsigned int j = 0; j < halfWidth; ++j) { - U = static_cast((*iU++ - 128) * 0.354); - U5 = 5 * U; - V = static_cast((*iV++ - 128) * 0.707); - V2 = 2 * V; - UV = -U - V; - Y0 = *yuv++; - Y1 = *yuv; - yuv = yuv + (width - 1); - Y2 = *yuv++; - Y3 = *yuv; - yuv = (yuv - width) + 1; - - // Original equations - // R = Y + 1.402 V - // G = Y - 0.344 U - 0.714 V - // B = Y + 1.772 U - R = Y0 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y0 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y0 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); - - //--- - R = Y1 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y1 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y1 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb = static_cast(B); - rgb = rgb + ((3 * width) - 5); - - //--- - R = Y2 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y2 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y2 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); - - //--- - R = Y3 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y3 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y3 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb = static_cast(B); - rgb = (rgb - (3 * width)) + 1; - } - yuv += width; - rgb += 3 * width; - } -} - -/*! - Convert YUV 4:2:0 [Y(NxM), U(N/2xM/2), V(N/2xM/2)] image into a grey image. - - \param[in] yuv : Pointer to the bitmap containing the YUV 4:2:0 data. - \param[out] grey : Pointer to the 8-bits grey bitmap that should be allocated with a size of width * height. - \param[in] size : Image size corresponding to image width * height. -*/ -void vpImageConvert::YUV420ToGrey(unsigned char *yuv, unsigned char *grey, unsigned int size) -{ - for (unsigned int i = 0; i < size; ++i) { - *grey++ = *yuv++; - } -} - -/*! - Convert YUV 4:4:4 (u y v) image into a RGBa image. - - The alpha component of the converted image is set to vpRGBa::alpha_default. - - \param[in] yuv : Pointer to the bitmap containing the YUV 4:4:4 data. - \param[out] rgba : Pointer to the 32-bits RGBA bitmap that should be allocated with a size of width * height * 4. - \param[in] size : Image size corresponding to image width * height. -*/ -void vpImageConvert::YUV444ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int size) -{ - for (unsigned int i = 0; i < size; ++i) { - int U = static_cast((*yuv++ - 128) * 0.354); - int U5 = 5 * U; - int Y = *yuv++; - int V = static_cast((*yuv++ - 128) * 0.707); - int V2 = 2 * V; - int UV = -U - V; - - // Original equations - // R = Y + 1.402 V - // G = Y - 0.344 U - 0.714 V - // B = Y + 1.772 U - int R = Y + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - int G = Y + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - int B = Y + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba++ = vpRGBa::alpha_default; - } -} - -/*! - Convert YUV 4:4:4 (u y v) image into RGB image. - - \param[in] yuv : Pointer to the bitmap containing the YUV 4:4:4 data. - \param[out] rgb : Pointer to the 24-bits RGB bitmap that should be allocated with a size of width * height * 3. - \param[in] size: Image size corresponding to image width * height. -*/ -void vpImageConvert::YUV444ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int size) -{ - for (unsigned int i = 0; i < size; ++i) { - int U = static_cast((*yuv++ - 128) * 0.354); - int U5 = 5 * U; - int Y = *yuv++; - int V = static_cast((*yuv++ - 128) * 0.707); - int V2 = 2 * V; - int UV = -U - V; - - // Original equations - // R = Y + 1.402 V - // G = Y - 0.344 U - 0.714 V - // B = Y + 1.772 U - int R = Y + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - int G = Y + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - int B = Y + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); - } -} - -/*! - Convert YUV 4:4:4 (u y v) image into a grey image. - - \param[in] yuv : Pointer to the bitmap containing the YUV 4:4:4 data. - \param[out] grey : Pointer to the 8-bits grey bitmap that should be allocated with a size of width * height. - \param[in] size: Image size corresponding to image width * height. -*/ -void vpImageConvert::YUV444ToGrey(unsigned char *yuv, unsigned char *grey, unsigned int size) -{ - ++yuv; - for (unsigned int i = 0; i < size; ++i) { - *grey++ = *yuv; - yuv = yuv + 3; - } -} - -/*! - Convert YV 1:2 [Y(NxM), V(N/2xM/2), U(N/2xM/2)] image into RGBa image. - - The alpha component of the converted image is set to vpRGBa::alpha_default. - - \param[in] yuv : Pointer to the bitmap containing the YV 1:2 data. - \param[out] rgba : Pointer to the 32-bits RGBA bitmap that should be allocated with a size of width * height * 4. - \param[in] width, height : Image size. - -*/ -void vpImageConvert::YV12ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int width, unsigned int height) -{ - // std::cout << "call optimized ConvertYV12ToRGBa()" << std::endl; - int U, V, R, G, B, V2, U5, UV; - int Y0, Y1, Y2, Y3; - unsigned int size = width * height; - unsigned char *iV = yuv + size; - unsigned char *iU = yuv + ((5 * size) / 4); - const unsigned int halfHeight = height / 2, halfWidth = width / 2; - for (unsigned int i = 0; i < halfHeight; ++i) { - for (unsigned int j = 0; j < halfWidth; ++j) { - U = static_cast((*iU++ - 128) * 0.354); - U5 = 5 * U; - V = static_cast((*iV++ - 128) * 0.707); - V2 = 2 * V; - UV = -U - V; - Y0 = *yuv++; - Y1 = *yuv; - yuv = yuv + (width - 1); - Y2 = *yuv++; - Y3 = *yuv; - yuv = (yuv - width) + 1; - - // Original equations - // R = Y + 1.402 V - // G = Y - 0.344 U - 0.714 V - // B = Y + 1.772 U - R = Y0 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y0 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y0 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba++ = vpRGBa::alpha_default; - - //--- - R = Y1 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y1 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y1 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba = 0; - rgba = rgba + ((4 * width) - 7); - - //--- - R = Y2 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y2 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y2 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba++ = vpRGBa::alpha_default; - - //--- - R = Y3 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y3 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y3 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba = vpRGBa::alpha_default; - rgba = (rgba - (4 * width)) + 1; - } - yuv += width; - rgba += 4 * width; - } -} - -/*! - Convert YV12 [Y(NxM), V(N/2xM/2), U(N/2xM/2)] image into RGB image. - - \param[in] yuv : Pointer to the bitmap containing the YV 1:2 data. - \param[out] rgb : Pointer to the 24-bits RGB bitmap that should be allocated with a size of width * height * 3. - \param[in] width, height : Image size. - -*/ -void vpImageConvert::YV12ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int height, unsigned int width) -{ - // std::cout << "call optimized ConvertYV12ToRGB()" << std::endl; - int U, V, R, G, B, V2, U5, UV; - int Y0, Y1, Y2, Y3; - unsigned int size = width * height; - unsigned char *iV = yuv + size; - unsigned char *iU = yuv + ((5 * size) / 4); - const unsigned int halfHeight = height / 2, halfWidth = width / 2; - for (unsigned int i = 0; i < halfHeight; ++i) { - for (unsigned int j = 0; j < halfWidth; ++j) { - U = static_cast((*iU++ - 128) * 0.354); - U5 = 5 * U; - V = static_cast((*iV++ - 128) * 0.707); - V2 = 2 * V; - UV = -U - V; - Y0 = *yuv++; - Y1 = *yuv; - yuv = yuv + (width - 1); - Y2 = *yuv++; - Y3 = *yuv; - yuv = (yuv - width) + 1; - - // Original equations - // R = Y + 1.402 V - // G = Y - 0.344 U - 0.714 V - // B = Y + 1.772 U - R = Y0 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y0 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y0 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); - - //--- - R = Y1 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y1 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y1 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb = static_cast(B); - rgb = rgb + ((3 * width) - 5); - - //--- - R = Y2 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y2 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y2 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); - - //--- - R = Y3 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y3 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y3 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb = static_cast(B); - rgb = (rgb - (3 * width)) + 1; - } - yuv += width; - rgb += 3 * width; - } -} - -/*! - Convert YVU 9 [Y(NxM), V(N/4xM/4), U(N/4xM/4)] image into a RGBa image. - - The alpha component of the converted image is set to vpRGBa::alpha_default. - - \param[in] yuv : Pointer to the bitmap containing the YVU 9 data. - \param[out] rgba : Pointer to the 32-bits RGBA bitmap that should be allocated with a size of width * height * 4. - \param[in] width, height : Image size. - -*/ -void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int width, unsigned int height) -{ - // std::cout << "call optimized ConvertYVU9ToRGBa()" << std::endl; - int U, V, R, G, B, V2, U5, UV; - int Y0, Y1, Y2, Y3, Y4, Y5, Y6, Y7, Y8, Y9, Y10, Y11, Y12, Y13, Y14, Y15; - unsigned int size = width * height; - unsigned char *iV = yuv + size; - unsigned char *iU = yuv + ((17 * size) / 16); - const unsigned int quarterHeight = height / 4, quarterWidth = width / 4; - for (unsigned int i = 0; i < quarterHeight; ++i) { - for (unsigned int j = 0; j < quarterWidth; ++j) { - U = static_cast((*iU++ - 128) * 0.354); - U5 = 5 * U; - V = static_cast((*iV++ - 128) * 0.707); - V2 = 2 * V; - UV = -U - V; - Y0 = *yuv++; - Y1 = *yuv++; - Y2 = *yuv++; - Y3 = *yuv; - yuv = yuv + (width - 3); - Y4 = *yuv++; - Y5 = *yuv++; - Y6 = *yuv++; - Y7 = *yuv; - yuv = yuv + (width - 3); - Y8 = *yuv++; - Y9 = *yuv++; - Y10 = *yuv++; - Y11 = *yuv; - yuv = yuv + (width - 3); - Y12 = *yuv++; - Y13 = *yuv++; - Y14 = *yuv++; - Y15 = *yuv; - yuv = (yuv - (3 * width)) + 1; - - // Original equations - // R = Y + 1.402 V - // G = Y - 0.344 U - 0.714 V - // B = Y + 1.772 U - R = Y0 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y0 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y0 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba++ = vpRGBa::alpha_default; - - //--- - R = Y1 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y1 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y1 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba++ = vpRGBa::alpha_default; - - //--- - R = Y2 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y2 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y2 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba++ = vpRGBa::alpha_default; - - //--- - R = Y3 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y3 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y3 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba = vpRGBa::alpha_default; - rgba = rgba + ((4 * width) - 15); - - R = Y4 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y4 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y4 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba++ = vpRGBa::alpha_default; - - //--- - R = Y5 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y5 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y5 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba++ = vpRGBa::alpha_default; - - //--- - R = Y6 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y6 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y6 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba++ = vpRGBa::alpha_default; - - //--- - R = Y7 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y7 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y7 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba = vpRGBa::alpha_default; - rgba = rgba + ((4 * width) - 15); - - R = Y8 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y8 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y8 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba++ = vpRGBa::alpha_default; - - //--- - R = Y9 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y9 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y9 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba++ = vpRGBa::alpha_default; - - //--- - R = Y10 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y10 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y10 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba++ = vpRGBa::alpha_default; - - //--- - R = Y11 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y11 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y11 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba = vpRGBa::alpha_default; - rgba = rgba + ((4 * width) - 15); - - R = Y12 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y12 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y12 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba++ = vpRGBa::alpha_default; - - //--- - R = Y13 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y13 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y13 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba++ = vpRGBa::alpha_default; - - //--- - R = Y14 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y14 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y14 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba++ = vpRGBa::alpha_default; - - //--- - R = Y15 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y15 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y15 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba = vpRGBa::alpha_default; - rgba = (rgba - (12 * width)) + 1; - } - yuv += 3 * width; - rgba += 12 * width; - } -} - -/*! - Convert YV 1:2 [Y(NxM), V(N/4xM/4), U(N/4xM/4)] image into RGB image. - - \param[in] yuv : Pointer to the bitmap containing the YV 1:2 data. - \param[out] rgb : Pointer to the 24-bits RGB bitmap that should be allocated with a size of width * height * 3. - \param[in] width, height : Image size. -*/ -void vpImageConvert::YVU9ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int height, unsigned int width) -{ - // std::cout << "call optimized ConvertYVU9ToRGB()" << std::endl; - int U, V, R, G, B, V2, U5, UV; - int Y0, Y1, Y2, Y3, Y4, Y5, Y6, Y7, Y8, Y9, Y10, Y11, Y12, Y13, Y14, Y15; - unsigned int size = width * height; - unsigned char *iV = yuv + size; - unsigned char *iU = yuv + ((17 * size) / 16); - const unsigned int quarterHeight = height / 4, quarterWidth = width / 4; - for (unsigned int i = 0; i < quarterHeight; ++i) { - for (unsigned int j = 0; j < quarterWidth; ++j) { - U = static_cast((*iU++ - 128) * 0.354); - U5 = 5 * U; - V = static_cast((*iV++ - 128) * 0.707); - V2 = 2 * V; - UV = -U - V; - Y0 = *yuv++; - Y1 = *yuv++; - Y2 = *yuv++; - Y3 = *yuv; - yuv = yuv + (width - 3); - Y4 = *yuv++; - Y5 = *yuv++; - Y6 = *yuv++; - Y7 = *yuv; - yuv = yuv + (width - 3); - Y8 = *yuv++; - Y9 = *yuv++; - Y10 = *yuv++; - Y11 = *yuv; - yuv = yuv + (width - 3); - Y12 = *yuv++; - Y13 = *yuv++; - Y14 = *yuv++; - Y15 = *yuv; - yuv = (yuv - (3 * width)) + 1; - - // Original equations - // R = Y + 1.402 V - // G = Y - 0.344 U - 0.714 V - // B = Y + 1.772 U - R = Y0 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y0 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y0 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); - - //--- - R = Y1 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y1 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y1 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); - - //--- - R = Y2 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y2 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y2 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); - - //--- - R = Y3 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y3 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y3 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb = static_cast(B); - rgb = rgb + ((3 * width) - 11); - - R = Y4 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y4 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y4 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); - - //--- - R = Y5 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y5 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y5 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); - - //--- - R = Y6 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y6 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y6 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); - - //--- - R = Y7 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y7 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y7 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb = static_cast(B); - rgb = rgb + ((3 * width) - 11); - - R = Y8 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y8 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y8 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); - - //--- - R = Y9 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y9 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y9 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); - - //--- - R = Y10 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y10 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y10 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); - - //--- - R = Y11 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y11 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y11 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb = static_cast(B); - rgb = rgb + 3 * width - 11; - - R = Y12 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y12 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y12 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); - - //--- - R = Y13 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y13 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y13 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); - - //--- - R = Y14 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y14 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y14 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); - - //--- - R = Y15 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } - - G = Y15 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } - - B = Y15 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); - rgb = (rgb - (9 * width)) + 1; - } - yuv += 3 * width; - rgb += 9 * width; - } -} - /*! Convert RGB into RGBa. diff --git a/modules/core/test/image-with-dataset/testColorConversion.cpp b/modules/core/test/image-with-dataset/testColorConversion.cpp index 3ee2ded944..87d18d3354 100644 --- a/modules/core/test/image-with-dataset/testColorConversion.cpp +++ b/modules/core/test/image-with-dataset/testColorConversion.cpp @@ -930,7 +930,7 @@ static double computePSNR(const vpImage &I_RGBA_8U, const vpImage &buffer) +static bool reaBinaryFile(const std::string &filename, std::vector &buffer) { std::FILE *f = std::fopen(filename.c_str(), "rb"); CHECK(f != nullptr); @@ -951,7 +951,7 @@ static bool readBinaryFile(const std::string &filename, std::vector &b return true; } -static bool readBinaryFile(const std::string &filename, std::vector &buffer) +static bool reaBinaryFile(const std::string &filename, std::vector &buffer) { std::FILE *f = std::fopen(filename.c_str(), "rb"); CHECK(f != nullptr); @@ -986,7 +986,7 @@ TEST_CASE("Bayer conversion", "[image_conversion]") { const std::string filename = vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "Bayer/Klimt_Bayer_560x558_BGGR_12bits.raw"); - if (readBinaryFile(filename, buffer)) { + if (reaBinaryFile(filename, buffer)) { col2im(buffer, I_Bayer_16U); SECTION("Bilinear") @@ -1017,7 +1017,7 @@ TEST_CASE("Bayer conversion", "[image_conversion]") { const std::string filename = vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "Bayer/Klimt_Bayer_560x558_GBRG_12bits.raw"); - if (readBinaryFile(filename, buffer)) { + if (reaBinaryFile(filename, buffer)) { col2im(buffer, I_Bayer_16U); SECTION("Bilinear") @@ -1048,7 +1048,7 @@ TEST_CASE("Bayer conversion", "[image_conversion]") { const std::string filename = vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "Bayer/Klimt_Bayer_560x558_GRBG_12bits.raw"); - if (readBinaryFile(filename, buffer)) { + if (reaBinaryFile(filename, buffer)) { col2im(buffer, I_Bayer_16U); SECTION("Bilinear") @@ -1079,7 +1079,7 @@ TEST_CASE("Bayer conversion", "[image_conversion]") { const std::string filename = vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "Bayer/Klimt_Bayer_560x558_RGGB_12bits.raw"); - if (readBinaryFile(filename, buffer)) { + if (reaBinaryFile(filename, buffer)) { col2im(buffer, I_Bayer_16U); SECTION("Bilinear") @@ -1118,7 +1118,7 @@ TEST_CASE("Bayer conversion", "[image_conversion]") const std::string filename = vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "Bayer/Klimt_Bayer_560x558_BGGR_08bits.raw"); - if (readBinaryFile(filename, buffer)) { + if (reaBinaryFile(filename, buffer)) { col2im(buffer, I_Bayer_8U); SECTION("Bilinear") @@ -1148,7 +1148,7 @@ TEST_CASE("Bayer conversion", "[image_conversion]") const std::string filename = vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "Bayer/Klimt_Bayer_560x558_GBRG_08bits.raw"); - if (readBinaryFile(filename, buffer)) { + if (reaBinaryFile(filename, buffer)) { col2im(buffer, I_Bayer_8U); SECTION("Bilinear") @@ -1178,7 +1178,7 @@ TEST_CASE("Bayer conversion", "[image_conversion]") const std::string filename = vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "Bayer/Klimt_Bayer_560x558_GRBG_08bits.raw"); - if (readBinaryFile(filename, buffer)) { + if (reaBinaryFile(filename, buffer)) { col2im(buffer, I_Bayer_8U); SECTION("Bilinear") @@ -1208,7 +1208,7 @@ TEST_CASE("Bayer conversion", "[image_conversion]") const std::string filename = vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "Bayer/Klimt_Bayer_560x558_RGGB_08bits.raw"); - if (readBinaryFile(filename, buffer)) { + if (reaBinaryFile(filename, buffer)) { col2im(buffer, I_Bayer_8U); SECTION("Bilinear") @@ -1451,6 +1451,89 @@ TEST_CASE("RGB to HSV conversion", "[image_conversion]") } } +// Inspired from https://stackoverflow.com/questions/17892346/how-to-convert-rgb-yuv-rgb-both-ways +void YUVToRGB(double Y, double U, double V, double &R, double &G, double &B) +{ + Y -= 16; + U -= 128; + V -= 128; + R = 1.164 * Y + 1.596 * V; + G = 1.164 * Y - 0.391 * U - 0.813 * V; + B = 1.164 * Y + 2.018 * U; + + R = R < 0. ? 0. : R; + G = G < 0. ? 0. : G; + B = B < 0. ? 0. : B; + R = R > 255. ? 255. : R; + G = G > 255. ? 255. : G; + B = B > 255. ? 255. : B; +} + +void RGBToYUV(const double R, const double G, const double B, double &Y, double &U, double &V) +{ + Y = 0.257 * R + 0.504 * G + 0.098 * B + 16; + U = -0.148 * R - 0.291 * G + 0.439 * B + 128; + V = 0.439 * R - 0.368 * G - 0.071 * B + 128; + + Y = Y < 0. ? 0. : Y; + U = U < 0. ? 0. : U; + V = V < 0. ? 0. : V; + Y = Y > 255. ? 255. : Y; + U = U > 255. ? 255. : U; + V = V > 255. ? 255. : V; +} + +TEST_CASE("YUV to RGB conversion", "[image_conversion]") +{ + std::vector< std::vector > rgb_truth; + rgb_truth.push_back({ 0, 0, 0 }); + rgb_truth.push_back({ 255, 255, 255 }); + rgb_truth.push_back({ 255, 0, 0 }); + rgb_truth.push_back({ 0, 255, 0 }); + rgb_truth.push_back({ 0, 0, 255 }); + rgb_truth.push_back({ 255, 255, 0 }); + rgb_truth.push_back({ 0, 255, 255 }); + rgb_truth.push_back({ 255, 0, 255 }); + rgb_truth.push_back({ 128, 128, 128 }); + rgb_truth.push_back({ 128, 128, 0 }); + rgb_truth.push_back({ 128, 0, 0 }); + rgb_truth.push_back({ 0, 128, 0 }); + rgb_truth.push_back({ 0, 128, 128 }); + rgb_truth.push_back({ 0, 0, 128 }); + rgb_truth.push_back({ 128, 0, 128 }); + + size_t size = rgb_truth.size(); + + std::vector< std::vector > yuv_truth; + for (size_t i = 0; i < size; ++i) { + double y, u, v; + double r = static_cast(rgb_truth[i][0]); + double g = static_cast(rgb_truth[i][1]); + double b = static_cast(rgb_truth[i][2]); + RGBToYUV(r, g, b, y, u, v); + yuv_truth.push_back({ y, u, v }); + } + SECTION("RGB -> YUV") + { + for (size_t i = 0; i < size; ++i) { + unsigned char r, g, b; + unsigned char y = static_cast(yuv_truth[i][0]); + unsigned char u = static_cast(yuv_truth[i][1]); + unsigned char v = static_cast(yuv_truth[i][2]); + + std::cout << "rgb truth (uchar ): " << (int)rgb_truth[i][0] << " " << (int)rgb_truth[i][1] << " " << (int)rgb_truth[i][2] << std::endl; + std::cout << "yuv truth (double): " << yuv_truth[i][0] << " " << yuv_truth[i][1] << " " << yuv_truth[i][2] << std::endl; + std::cout << "yuv truth (uchar ): " << (int)y << " " << (int)u << " " << (int)v << std::endl; + vpImageConvert::YUVToRGB(y, u, v, r, g, b); + std::cout << "rgb (uchar ): " << (int)r << " " << (int)g << " " << (int)b << std::endl; + double r_truth, g_truth, b_truth; + YUVToRGB(yuv_truth[i][0], yuv_truth[i][1], yuv_truth[i][2], r_truth, g_truth, b_truth); + std::cout << "rgb (double): " << r_truth << " " << g_truth << " " << b_truth << std::endl; + + } + } +} + int main(int argc, char *argv[]) { Catch::Session session; // There must be exactly one instance From 496280b6eb128beb9fc6715d21f4e08aa08db3ae Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Mon, 8 Apr 2024 10:02:17 +0200 Subject: [PATCH 05/17] Satisfy quality rules by have files with less than 3000 lines --- modules/core/src/image/vpImageConvert_yuv.cpp | 2637 +++++++++++++++++ 1 file changed, 2637 insertions(+) create mode 100644 modules/core/src/image/vpImageConvert_yuv.cpp diff --git a/modules/core/src/image/vpImageConvert_yuv.cpp b/modules/core/src/image/vpImageConvert_yuv.cpp new file mode 100644 index 0000000000..2291700634 --- /dev/null +++ b/modules/core/src/image/vpImageConvert_yuv.cpp @@ -0,0 +1,2637 @@ +/**************************************************************************** + * + * ViSP, open source Visual Servoing Platform software. + * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * + * This software is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * See the file LICENSE.txt at the root directory of this source + * distribution for additional information about the GNU GPL. + * + * For using ViSP with software that can not be combined with the GNU + * GPL, please contact Inria about acquiring a ViSP Professional + * Edition License. + * + * See https://visp.inria.fr for more information. + * + * This software was developed at: + * Inria Rennes - Bretagne Atlantique + * Campus Universitaire de Beaulieu + * 35042 Rennes Cedex + * France + * + * If you have questions regarding the use of this file, please contact + * Inria at visp@inria.fr + * + * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE + * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. + * + * Description: + * Convert image types. + * +*****************************************************************************/ + +/*! + \file vpImageConvert_yuv.cpp + \brief Various yuv formats to RGB and RGBa conversion. +*/ + + +#include +#include + +#define vpSAT(c) \ + if (c & (~255)) { \ + if (c < 0) \ + {c = 0;} \ + else \ + {c = 255;} \ + } +/*! + Convert an image from YUYV 4:2:2 (y0 u01 y1 v01 y2 u23 y3 v23 ...) to RGB32. + Destination rgba memory area has to be allocated before. + + The alpha component of the converted image is set to vpRGBa::alpha_default. + + \param[in] yuyv : Pointer to the bitmap containing the YUYV 4:2:2 data. + \param[out] rgba : Pointer to the RGB32 bitmap that should be allocated with a size of \e width * \e height * 4. + \param[in] width, height : Image size. + + \sa YUV422ToRGBa() +*/ +void vpImageConvert::YUYVToRGBa(unsigned char *yuyv, unsigned char *rgba, unsigned int width, unsigned int height) +{ + unsigned char *s; + unsigned char *d; + int w, h; + int r, g, b, cr, cg, cb, y1, y2; + + h = static_cast(height); + w = static_cast(width); + s = yuyv; + d = rgba; + while (h--) { + int c = w >> 1; + while (c--) { + y1 = *s++; + cb = ((*s - 128) * 454) >> 8; + cg = (*s++ - 128) * 88; + y2 = *s++; + cr = ((*s - 128) * 359) >> 8; + cg = (cg + ((*s++ - 128) * 183)) >> 8; + + r = y1 + cr; + b = y1 + cb; + g = y1 - cg; + vpSAT(r) vpSAT(g) vpSAT(b) + + *d++ = static_cast(r); + *d++ = static_cast(g); + *d++ = static_cast(b); + *d++ = vpRGBa::alpha_default; + + r = y2 + cr; + b = y2 + cb; + g = y2 - cg; + vpSAT(r) vpSAT(g) vpSAT(b) + + *d++ = static_cast(r); + *d++ = static_cast(g); + *d++ = static_cast(b); + *d++ = vpRGBa::alpha_default; + } + } +} + +/*! + Convert an image from YUYV 4:2:2 (y0 u01 y1 v01 y2 u23 y3 v23 ...) + to RGB24. Destination rgb memory area has to be allocated before. + + \param[in] yuyv : Pointer to the bitmap containing the YUYV 4:2:2 data. + \param[out] rgb : Pointer to the RGB32 bitmap that should be allocated with a size of \e width * \e height * 3. + \param[in] width, height : Image size. + + \sa YUV422ToRGB() +*/ +void vpImageConvert::YUYVToRGB(unsigned char *yuyv, unsigned char *rgb, unsigned int width, unsigned int height) +{ + unsigned char *s; + unsigned char *d; + int h, w; + int r, g, b, cr, cg, cb, y1, y2; + + h = static_cast(height); + w = static_cast(width); + s = yuyv; + d = rgb; + while (h--) { + int c = w >> 1; + while (c--) { + y1 = *s++; + cb = ((*s - 128) * 454) >> 8; + cg = (*s++ - 128) * 88; + y2 = *s++; + cr = ((*s - 128) * 359) >> 8; + cg = (cg + ((*s++ - 128) * 183)) >> 8; + + r = y1 + cr; + b = y1 + cb; + g = y1 - cg; + vpSAT(r) vpSAT(g) vpSAT(b) + + *d++ = static_cast(r); + *d++ = static_cast(g); + *d++ = static_cast(b); + + r = y2 + cr; + b = y2 + cb; + g = y2 - cg; + vpSAT(r) vpSAT(g) vpSAT(b) + + *d++ = static_cast(r); + *d++ = static_cast(g); + *d++ = static_cast(b); + } + } +} + +/*! + Convert an image from YUYV 4:2:2 (y0 u01 y1 v01 y2 u23 y3 v23 ...) + to grey. Destination grey memory area has to be allocated before. + + \param[in] yuyv : Pointer to the bitmap containing the YUYV 4:2:2 data. + \param[out] grey : Pointer to the 8-bits grey bitmap that should be allocated with a size of width * height. + \param[in] size : Image size corresponding to width * height. + + \sa YUV422ToGrey() +*/ +void vpImageConvert::YUYVToGrey(unsigned char *yuyv, unsigned char *grey, unsigned int size) +{ + unsigned int i = 0, j = 0; + const unsigned int doubleSize = size * 2; + while (j < doubleSize) { + grey[i++] = yuyv[j]; + grey[i++] = yuyv[j + 2]; + j += 4; + } +} + +/*! + Convert YUV 4:1:1 (u y1 y2 v y3 y4) images into RGBa images. The alpha + component of the converted image is set to vpRGBa::alpha_default. + + \param[in] yuv : Pointer to the bitmap containing the YUV 4:1:1 data. + \param[out] rgba : Pointer to the RGBA 32-bits bitmap that should be allocated with a size of width * height * 4. + \param[in] size : Image size corresponding to width * height. + +*/ +void vpImageConvert::YUV411ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int size) +{ +#if 1 + // std::cout << "call optimized ConvertYUV411ToRGBa()" << std::endl; + for (unsigned int i = size / 4; i; --i) { + int U = static_cast((*yuv++ - 128) * 0.354); + int U5 = 5 * U; + int Y0 = *yuv++; + int Y1 = *yuv++; + int V = static_cast((*yuv++ - 128) * 0.707); + int V2 = 2 * V; + int Y2 = *yuv++; + int Y3 = *yuv++; + int UV = -U - V; + + // Original equations + // R = Y + 1.402 V + // G = Y - 0.344 U - 0.714 V + // B = Y + 1.772 U + int R = Y0 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + int G = Y0 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + int B = Y0 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgba++ = static_cast(R); + *rgba++ = static_cast(G); + *rgba++ = static_cast(B); + *rgba++ = vpRGBa::alpha_default; + + //--- + R = Y1 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y1 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y1 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgba++ = static_cast(R); + *rgba++ = static_cast(G); + *rgba++ = static_cast(B); + *rgba++ = vpRGBa::alpha_default; + + //--- + R = Y2 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y2 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y2 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgba++ = static_cast(R); + *rgba++ = static_cast(G); + *rgba++ = static_cast(B); + *rgba++ = vpRGBa::alpha_default; + + //--- + R = Y3 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y3 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y3 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgba++ = static_cast(R); + *rgba++ = static_cast(G); + *rgba++ = static_cast(B); + *rgba++ = vpRGBa::alpha_default; + } +#else + // tres tres lent .... + unsigned int i = 0, j = 0; + unsigned char r, g, b; + const unsigned int iterLimit = (numpixels * 3) / 2; + while (j < iterLimit) { + + YUVToRGB(yuv[j + 1], yuv[j], yuv[j + 3], r, g, b); + rgba[i] = r; + rgba[i + 1] = g; + rgba[i + 2] = b; + rgba[i + 3] = vpRGBa::alpha_default; + i += 4; + + YUVToRGB(yuv[j + 2], yuv[j], yuv[j + 3], r, g, b); + rgba[i] = r; + rgba[i + 1] = g; + rgba[i + 2] = b; + rgba[i + 3] = vpRGBa::alpha_default; + i += 4; + + YUVToRGB(yuv[j + 4], yuv[j], yuv[j + 3], r, g, b); + rgba[i] = r; + rgba[i + 1] = g; + rgba[i + 2] = b; + rgba[i + 3] = vpRGBa::alpha_default; + i += 4; + + YUVToRGB(yuv[j + 5], yuv[j], yuv[j + 3], r, g, b); + rgba[i] = r; + rgba[i + 1] = g; + rgba[i + 2] = b; + rgba[i + 3] = vpRGBa::alpha_default; + i += 4; + + j += 6; + } +#endif +} + +/*! + Convert YUV 4:2:2 (u01 y0 v01 y1 u23 y2 v23 y3 ...) images into RGBa images. + Destination rgba memory area has to be allocated before. + + The alpha component of the converted image is set to vpRGBa::alpha_default. + + \param[in] yuv : Pointer to the bitmap containing the YUV 4:2:2 data. + \param[out] rgba : Pointer to the RGBA 32-bits bitmap that should be allocated with a size of width * height * 4. + \param[in] size : Image size corresponding to width * height. + + \sa YUYVToRGBa() +*/ +void vpImageConvert::YUV422ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int size) +{ + +#if 1 + // std::cout << "call optimized convertYUV422ToRGBa()" << std::endl; + for (unsigned int i = size / 2; i; --i) { + int U = static_cast((*yuv++ - 128) * 0.354); + int U5 = 5 * U; + int Y0 = *yuv++; + int V = static_cast((*yuv++ - 128) * 0.707); + int V2 = 2 * V; + int Y1 = *yuv++; + int UV = -U - V; + + //--- + int R = Y0 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + int G = Y0 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + int B = Y0 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgba++ = static_cast(R); + *rgba++ = static_cast(G); + *rgba++ = static_cast(B); + *rgba++ = vpRGBa::alpha_default; + + //--- + R = Y1 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y1 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y1 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgba++ = static_cast(R); + *rgba++ = static_cast(G); + *rgba++ = static_cast(B); + *rgba++ = vpRGBa::alpha_default; + } + +#else + // tres tres lent .... + unsigned int i = 0, j = 0; + unsigned char r, g, b; + const unsigned int doubleSize = size * 2; + while (j < doubleSize) { + + YUVToRGB(yuv[j + 1], yuv[j], yuv[j + 2], r, g, b); + rgba[i] = r; + rgba[i + 1] = g; + rgba[i + 2] = b; + rgba[i + 3] = vpRGBa::alpha_default; + i += 4; + + YUVToRGB(yuv[j + 3], yuv[j], yuv[j + 2], r, g, b); + rgba[i] = r; + rgba[i + 1] = g; + rgba[i + 2] = b; + rgba[i + 3] = vpRGBa::alpha_default; + i += 4; + j += 4; + } +#endif +} + +/*! + Convert YUV 4:1:1 (u y1 y2 v y3 y4) into a grey image. + + \param[in] yuv : Pointer to the bitmap containing the YUV 4:1:1 data. + \param[out] grey : Pointer to the 8-bits grey bitmap that should be allocated with a size of width * height. + \param[in] size : Image size corresponding to width * height. + +*/ +void vpImageConvert::YUV411ToGrey(unsigned char *yuv, unsigned char *grey, unsigned int size) +{ + unsigned int i = 0, j = 0; + const unsigned int iterLimit = (size * 3) / 2; + while (j < iterLimit) { + grey[i] = yuv[j + 1]; + grey[i + 1] = yuv[j + 2]; + grey[i + 2] = yuv[j + 4]; + grey[i + 3] = yuv[j + 5]; + + i += 4; + + j += 6; + } +} + +/*! + Convert YUV 4:2:2 (u01 y0 v01 y1 u23 y2 v23 y3 ...) images into RGB images. + Destination rgb memory area has to be allocated before. + + \param[in] yuv : Pointer to the bitmap containing the YUV 4:2:2 data. + \param[out] rgb : Pointer to the 24-bits RGB bitmap that should be allocated with a size of width * height * 3. + \param[in] size : Image size corresponding to width * height. + + \sa YUYVToRGB() +*/ +void vpImageConvert::YUV422ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int size) +{ +#if 1 + // std::cout << "call optimized convertYUV422ToRGB()" << std::endl; + for (unsigned int i = size / 2; i; --i) { + int U = static_cast((*yuv++ - 128) * 0.354); + int U5 = 5 * U; + int Y0 = *yuv++; + int V = static_cast((*yuv++ - 128) * 0.707); + int V2 = 2 * V; + int Y1 = *yuv++; + int UV = -U - V; + + //--- + int R = Y0 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + int G = Y0 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + int B = Y0 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgb++ = static_cast(R); + *rgb++ = static_cast(G); + *rgb++ = static_cast(B); + + //--- + R = Y1 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y1 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y1 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgb++ = static_cast(R); + *rgb++ = static_cast(G); + *rgb++ = static_cast(B); + } + +#else + // tres tres lent .... + unsigned int i = 0, j = 0; + unsigned char r, g, b; + const unsigned int doubleSize = size * 2; + while (j < doubleSize) { + + YUVToRGB(yuv[j + 1], yuv[j], yuv[j + 2], r, g, b); + rgb[i] = r; + rgb[i + 1] = g; + rgb[i + 2] = b; + i += 3; + + YUVToRGB(yuv[j + 3], yuv[j], yuv[j + 2], r, g, b); + rgb[i] = r; + rgb[i + 1] = g; + rgb[i + 2] = b; + i += 3; + j += 4; + } +#endif +} + +/*! + Convert YUV 4:2:2 (u01 y0 v01 y1 u23 y2 v23 y3 ...) images into a grey image. + Destination grey memory area has to be allocated before. + + \param[in] yuv : Pointer to the bitmap containing the YUV 4:2:2 data. + \param[out] grey : Pointer to the 8-bits grey bitmap that should be allocated with a size of width * height. + \param[in] size : Image size corresponding to width * height. + + \sa YUYVToGrey() +*/ +void vpImageConvert::YUV422ToGrey(unsigned char *yuv, unsigned char *grey, unsigned int size) +{ + unsigned int i = 0, j = 0; + const unsigned int doubleSize = size * 2; + + while (j < doubleSize) { + grey[i++] = yuv[j + 1]; + grey[i++] = yuv[j + 3]; + j += 4; + } +} + +/*! + Convert YUV 4:1:1 (u y1 y2 v y3 y4) into a RGB 24bits image. + + \param[in] yuv : Pointer to the bitmap containing the YUV 4:1:1 data. + \param[out] rgb : Pointer to the 24-bits RGB bitmap that should be allocated with a size of width * height * 3. + \param[in] size : Image size corresponding to width * height. + +*/ +void vpImageConvert::YUV411ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int size) +{ +#if 1 + // std::cout << "call optimized ConvertYUV411ToRGB()" << std::endl; + for (unsigned int i = size / 4; i; --i) { + int U = static_cast((*yuv++ - 128) * 0.354); + int U5 = 5 * U; + int Y0 = *yuv++; + int Y1 = *yuv++; + int V = static_cast((*yuv++ - 128) * 0.707); + int V2 = 2 * V; + int Y2 = *yuv++; + int Y3 = *yuv++; + int UV = -U - V; + + // Original equations + // R = Y + 1.402 V + // G = Y - 0.344 U - 0.714 V + // B = Y + 1.772 U + int R = Y0 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + int G = Y0 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + int B = Y0 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgb++ = static_cast(R); + *rgb++ = static_cast(G); + *rgb++ = static_cast(B); + + //--- + R = Y1 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y1 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y1 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgb++ = static_cast(R); + *rgb++ = static_cast(G); + *rgb++ = static_cast(B); + + //--- + R = Y2 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y2 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y2 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgb++ = static_cast(R); + *rgb++ = static_cast(G); + *rgb++ = static_cast(B); + + //--- + R = Y3 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y3 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y3 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgb++ = static_cast(R); + *rgb++ = static_cast(G); + *rgb++ = static_cast(B); + } +#else + // tres tres lent .... + + unsigned int i = 0, j = 0; + unsigned char r, g, b; + + const unsigned int iterLimit = (size * 3) / 2; + while (j < iterLimit) { + YUVToRGB(yuv[j + 1], yuv[j], yuv[j + 3], r, g, b); + rgb[i] = r; + rgb[i + 1] = g; + rgb[i + 2] = b; + i += 3; + + YUVToRGB(yuv[j + 2], yuv[j], yuv[j + 3], r, g, b); + rgb[i] = r; + rgb[i + 1] = g; + rgb[i + 2] = b; + i += 3; + + YUVToRGB(yuv[j + 4], yuv[j], yuv[j + 3], r, g, b); + rgb[i] = r; + rgb[i + 1] = g; + rgb[i + 2] = b; + i += 3; + + YUVToRGB(yuv[j + 5], yuv[j], yuv[j + 3], r, g, b); + rgb[i] = r; + rgb[i + 1] = g; + rgb[i + 2] = b; + i += 3; + // TRACE("r= %d g=%d b=%d", r, g, b); + + j += 6; + } +#endif +} + +/*! + Convert YUV 4:2:0 [Y(NxM), U(N/2xM/2), V(N/2xM/2)] image into a RGBa image. + + The alpha component of the converted image is set to vpRGBa::alpha_default. + + \param[in] yuv : Pointer to the bitmap containing the YUV 4:2:0 data. + \param[out] rgba : Pointer to the 32-bits RGBA bitmap that should be allocated with a size of width * height * 4. + \param[in] width, height : Image size. + +*/ +void vpImageConvert::YUV420ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int width, unsigned int height) +{ + // std::cout << "call optimized ConvertYUV420ToRGBa()" << std::endl; + int U, V, R, G, B, V2, U5, UV; + int Y0, Y1, Y2, Y3; + unsigned int size = width * height; + unsigned char *iU = yuv + size; + unsigned char *iV = yuv + ((5 * size) / 4); + const unsigned int halfHeight = height / 2, halfWidth = width / 2; + for (unsigned int i = 0; i < halfHeight; ++i) { + for (unsigned int j = 0; j < halfWidth; ++j) { + U = static_cast((*iU++ - 128) * 0.354); + U5 = 5 * U; + V = static_cast((*iV++ - 128) * 0.707); + V2 = 2 * V; + UV = -U - V; + Y0 = *yuv++; + Y1 = *yuv; + yuv = yuv + (width - 1); + Y2 = *yuv++; + Y3 = *yuv; + yuv = (yuv - width) + 1; + + // Original equations + // R = Y + 1.402 V + // G = Y - 0.344 U - 0.714 V + // B = Y + 1.772 U + R = Y0 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y0 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y0 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgba++ = static_cast(R); + *rgba++ = static_cast(G); + *rgba++ = static_cast(B); + *rgba++ = vpRGBa::alpha_default; + + //--- + R = Y1 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y1 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y1 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgba++ = static_cast(R); + *rgba++ = static_cast(G); + *rgba++ = static_cast(B); + *rgba = vpRGBa::alpha_default; + rgba = rgba + 4 * width - 7; + + //--- + R = Y2 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y2 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y2 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgba++ = static_cast(R); + *rgba++ = static_cast(G); + *rgba++ = static_cast(B); + *rgba++ = vpRGBa::alpha_default; + + //--- + R = Y3 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y3 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y3 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgba++ = static_cast(R); + *rgba++ = static_cast(G); + *rgba++ = static_cast(B); + *rgba = vpRGBa::alpha_default; + rgba = (rgba - (4 * width)) + 1; + } + yuv += width; + rgba += 4 * width; + } +} + +/*! + Convert YUV 4:2:0 [Y(NxM), U(N/2xM/2), V(N/2xM/2)] image into a RGB image. + + \param[in] yuv : Pointer to the bitmap containing the YUV 4:2:0 data. + \param[out] rgb : Pointer to the 24-bits RGB bitmap that should be allocated with a size of width * height * 3. + \param[in] width, height : Image size. + +*/ +void vpImageConvert::YUV420ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int width, unsigned int height) +{ + // std::cout << "call optimized ConvertYUV420ToRGB()" << std::endl; + int U, V, R, G, B, V2, U5, UV; + int Y0, Y1, Y2, Y3; + unsigned int size = width * height; + unsigned char *iU = yuv + size; + unsigned char *iV = yuv + ((5 * size) / 4); + const unsigned int halfHeight = height / 2, halfWidth = width / 2; + for (unsigned int i = 0; i < halfHeight; ++i) { + for (unsigned int j = 0; j < halfWidth; ++j) { + U = static_cast((*iU++ - 128) * 0.354); + U5 = 5 * U; + V = static_cast((*iV++ - 128) * 0.707); + V2 = 2 * V; + UV = -U - V; + Y0 = *yuv++; + Y1 = *yuv; + yuv = yuv + (width - 1); + Y2 = *yuv++; + Y3 = *yuv; + yuv = (yuv - width) + 1; + + // Original equations + // R = Y + 1.402 V + // G = Y - 0.344 U - 0.714 V + // B = Y + 1.772 U + R = Y0 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y0 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y0 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgb++ = static_cast(R); + *rgb++ = static_cast(G); + *rgb++ = static_cast(B); + + //--- + R = Y1 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y1 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y1 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgb++ = static_cast(R); + *rgb++ = static_cast(G); + *rgb = static_cast(B); + rgb = rgb + ((3 * width) - 5); + + //--- + R = Y2 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y2 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y2 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgb++ = static_cast(R); + *rgb++ = static_cast(G); + *rgb++ = static_cast(B); + + //--- + R = Y3 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y3 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y3 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgb++ = static_cast(R); + *rgb++ = static_cast(G); + *rgb = static_cast(B); + rgb = (rgb - (3 * width)) + 1; + } + yuv += width; + rgb += 3 * width; + } +} + +/*! + Convert YUV 4:2:0 [Y(NxM), U(N/2xM/2), V(N/2xM/2)] image into a grey image. + + \param[in] yuv : Pointer to the bitmap containing the YUV 4:2:0 data. + \param[out] grey : Pointer to the 8-bits grey bitmap that should be allocated with a size of width * height. + \param[in] size : Image size corresponding to image width * height. +*/ +void vpImageConvert::YUV420ToGrey(unsigned char *yuv, unsigned char *grey, unsigned int size) +{ + for (unsigned int i = 0; i < size; ++i) { + *grey++ = *yuv++; + } +} + +/*! + Convert YUV 4:4:4 (u y v) image into a RGBa image. + + The alpha component of the converted image is set to vpRGBa::alpha_default. + + \param[in] yuv : Pointer to the bitmap containing the YUV 4:4:4 data. + \param[out] rgba : Pointer to the 32-bits RGBA bitmap that should be allocated with a size of width * height * 4. + \param[in] size : Image size corresponding to image width * height. +*/ +void vpImageConvert::YUV444ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int size) +{ + for (unsigned int i = 0; i < size; ++i) { + int U = static_cast((*yuv++ - 128) * 0.354); + int U5 = 5 * U; + int Y = *yuv++; + int V = static_cast((*yuv++ - 128) * 0.707); + int V2 = 2 * V; + int UV = -U - V; + + // Original equations + // R = Y + 1.402 V + // G = Y - 0.344 U - 0.714 V + // B = Y + 1.772 U + int R = Y + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + int G = Y + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + int B = Y + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgba++ = static_cast(R); + *rgba++ = static_cast(G); + *rgba++ = static_cast(B); + *rgba++ = vpRGBa::alpha_default; + } +} + +/*! + Convert YUV 4:4:4 (u y v) image into RGB image. + + \param[in] yuv : Pointer to the bitmap containing the YUV 4:4:4 data. + \param[out] rgb : Pointer to the 24-bits RGB bitmap that should be allocated with a size of width * height * 3. + \param[in] size: Image size corresponding to image width * height. +*/ +void vpImageConvert::YUV444ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int size) +{ + for (unsigned int i = 0; i < size; ++i) { + int U = static_cast((*yuv++ - 128) * 0.354); + int U5 = 5 * U; + int Y = *yuv++; + int V = static_cast((*yuv++ - 128) * 0.707); + int V2 = 2 * V; + int UV = -U - V; + + // Original equations + // R = Y + 1.402 V + // G = Y - 0.344 U - 0.714 V + // B = Y + 1.772 U + int R = Y + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + int G = Y + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + int B = Y + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgb++ = static_cast(R); + *rgb++ = static_cast(G); + *rgb++ = static_cast(B); + } +} + +/*! + Convert YUV 4:4:4 (u y v) image into a grey image. + + \param[in] yuv : Pointer to the bitmap containing the YUV 4:4:4 data. + \param[out] grey : Pointer to the 8-bits grey bitmap that should be allocated with a size of width * height. + \param[in] size: Image size corresponding to image width * height. +*/ +void vpImageConvert::YUV444ToGrey(unsigned char *yuv, unsigned char *grey, unsigned int size) +{ + ++yuv; + for (unsigned int i = 0; i < size; ++i) { + *grey++ = *yuv; + yuv = yuv + 3; + } +} + +/*! + Convert YV 1:2 [Y(NxM), V(N/2xM/2), U(N/2xM/2)] image into RGBa image. + + The alpha component of the converted image is set to vpRGBa::alpha_default. + + \param[in] yuv : Pointer to the bitmap containing the YV 1:2 data. + \param[out] rgba : Pointer to the 32-bits RGBA bitmap that should be allocated with a size of width * height * 4. + \param[in] width, height : Image size. + +*/ +void vpImageConvert::YV12ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int width, unsigned int height) +{ + // std::cout << "call optimized ConvertYV12ToRGBa()" << std::endl; + int U, V, R, G, B, V2, U5, UV; + int Y0, Y1, Y2, Y3; + unsigned int size = width * height; + unsigned char *iV = yuv + size; + unsigned char *iU = yuv + ((5 * size) / 4); + const unsigned int halfHeight = height / 2, halfWidth = width / 2; + for (unsigned int i = 0; i < halfHeight; ++i) { + for (unsigned int j = 0; j < halfWidth; ++j) { + U = static_cast((*iU++ - 128) * 0.354); + U5 = 5 * U; + V = static_cast((*iV++ - 128) * 0.707); + V2 = 2 * V; + UV = -U - V; + Y0 = *yuv++; + Y1 = *yuv; + yuv = yuv + (width - 1); + Y2 = *yuv++; + Y3 = *yuv; + yuv = (yuv - width) + 1; + + // Original equations + // R = Y + 1.402 V + // G = Y - 0.344 U - 0.714 V + // B = Y + 1.772 U + R = Y0 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y0 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y0 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgba++ = static_cast(R); + *rgba++ = static_cast(G); + *rgba++ = static_cast(B); + *rgba++ = vpRGBa::alpha_default; + + //--- + R = Y1 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y1 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y1 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgba++ = static_cast(R); + *rgba++ = static_cast(G); + *rgba++ = static_cast(B); + *rgba = 0; + rgba = rgba + ((4 * width) - 7); + + //--- + R = Y2 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y2 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y2 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgba++ = static_cast(R); + *rgba++ = static_cast(G); + *rgba++ = static_cast(B); + *rgba++ = vpRGBa::alpha_default; + + //--- + R = Y3 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y3 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y3 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgba++ = static_cast(R); + *rgba++ = static_cast(G); + *rgba++ = static_cast(B); + *rgba = vpRGBa::alpha_default; + rgba = (rgba - (4 * width)) + 1; + } + yuv += width; + rgba += 4 * width; + } +} + +/*! + Convert YV12 [Y(NxM), V(N/2xM/2), U(N/2xM/2)] image into RGB image. + + \param[in] yuv : Pointer to the bitmap containing the YV 1:2 data. + \param[out] rgb : Pointer to the 24-bits RGB bitmap that should be allocated with a size of width * height * 3. + \param[in] width, height : Image size. + +*/ +void vpImageConvert::YV12ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int height, unsigned int width) +{ + // std::cout << "call optimized ConvertYV12ToRGB()" << std::endl; + int U, V, R, G, B, V2, U5, UV; + int Y0, Y1, Y2, Y3; + unsigned int size = width * height; + unsigned char *iV = yuv + size; + unsigned char *iU = yuv + ((5 * size) / 4); + const unsigned int halfHeight = height / 2, halfWidth = width / 2; + for (unsigned int i = 0; i < halfHeight; ++i) { + for (unsigned int j = 0; j < halfWidth; ++j) { + U = static_cast((*iU++ - 128) * 0.354); + U5 = 5 * U; + V = static_cast((*iV++ - 128) * 0.707); + V2 = 2 * V; + UV = -U - V; + Y0 = *yuv++; + Y1 = *yuv; + yuv = yuv + (width - 1); + Y2 = *yuv++; + Y3 = *yuv; + yuv = (yuv - width) + 1; + + // Original equations + // R = Y + 1.402 V + // G = Y - 0.344 U - 0.714 V + // B = Y + 1.772 U + R = Y0 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y0 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y0 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgb++ = static_cast(R); + *rgb++ = static_cast(G); + *rgb++ = static_cast(B); + + //--- + R = Y1 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y1 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y1 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgb++ = static_cast(R); + *rgb++ = static_cast(G); + *rgb = static_cast(B); + rgb = rgb + ((3 * width) - 5); + + //--- + R = Y2 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y2 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y2 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgb++ = static_cast(R); + *rgb++ = static_cast(G); + *rgb++ = static_cast(B); + + //--- + R = Y3 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y3 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y3 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgb++ = static_cast(R); + *rgb++ = static_cast(G); + *rgb = static_cast(B); + rgb = (rgb - (3 * width)) + 1; + } + yuv += width; + rgb += 3 * width; + } +} + +/*! + Convert YVU 9 [Y(NxM), V(N/4xM/4), U(N/4xM/4)] image into a RGBa image. + + The alpha component of the converted image is set to vpRGBa::alpha_default. + + \param[in] yuv : Pointer to the bitmap containing the YVU 9 data. + \param[out] rgba : Pointer to the 32-bits RGBA bitmap that should be allocated with a size of width * height * 4. + \param[in] width, height : Image size. + +*/ +void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int width, unsigned int height) +{ + // std::cout << "call optimized ConvertYVU9ToRGBa()" << std::endl; + int U, V, R, G, B, V2, U5, UV; + int Y0, Y1, Y2, Y3, Y4, Y5, Y6, Y7, Y8, Y9, Y10, Y11, Y12, Y13, Y14, Y15; + unsigned int size = width * height; + unsigned char *iV = yuv + size; + unsigned char *iU = yuv + ((17 * size) / 16); + const unsigned int quarterHeight = height / 4, quarterWidth = width / 4; + for (unsigned int i = 0; i < quarterHeight; ++i) { + for (unsigned int j = 0; j < quarterWidth; ++j) { + U = static_cast((*iU++ - 128) * 0.354); + U5 = 5 * U; + V = static_cast((*iV++ - 128) * 0.707); + V2 = 2 * V; + UV = -U - V; + Y0 = *yuv++; + Y1 = *yuv++; + Y2 = *yuv++; + Y3 = *yuv; + yuv = yuv + (width - 3); + Y4 = *yuv++; + Y5 = *yuv++; + Y6 = *yuv++; + Y7 = *yuv; + yuv = yuv + (width - 3); + Y8 = *yuv++; + Y9 = *yuv++; + Y10 = *yuv++; + Y11 = *yuv; + yuv = yuv + (width - 3); + Y12 = *yuv++; + Y13 = *yuv++; + Y14 = *yuv++; + Y15 = *yuv; + yuv = (yuv - (3 * width)) + 1; + + // Original equations + // R = Y + 1.402 V + // G = Y - 0.344 U - 0.714 V + // B = Y + 1.772 U + R = Y0 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y0 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y0 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgba++ = static_cast(R); + *rgba++ = static_cast(G); + *rgba++ = static_cast(B); + *rgba++ = vpRGBa::alpha_default; + + //--- + R = Y1 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y1 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y1 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgba++ = static_cast(R); + *rgba++ = static_cast(G); + *rgba++ = static_cast(B); + *rgba++ = vpRGBa::alpha_default; + + //--- + R = Y2 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y2 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y2 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgba++ = static_cast(R); + *rgba++ = static_cast(G); + *rgba++ = static_cast(B); + *rgba++ = vpRGBa::alpha_default; + + //--- + R = Y3 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y3 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y3 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgba++ = static_cast(R); + *rgba++ = static_cast(G); + *rgba++ = static_cast(B); + *rgba = vpRGBa::alpha_default; + rgba = rgba + ((4 * width) - 15); + + R = Y4 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y4 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y4 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgba++ = static_cast(R); + *rgba++ = static_cast(G); + *rgba++ = static_cast(B); + *rgba++ = vpRGBa::alpha_default; + + //--- + R = Y5 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y5 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y5 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgba++ = static_cast(R); + *rgba++ = static_cast(G); + *rgba++ = static_cast(B); + *rgba++ = vpRGBa::alpha_default; + + //--- + R = Y6 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y6 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y6 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgba++ = static_cast(R); + *rgba++ = static_cast(G); + *rgba++ = static_cast(B); + *rgba++ = vpRGBa::alpha_default; + + //--- + R = Y7 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y7 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y7 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgba++ = static_cast(R); + *rgba++ = static_cast(G); + *rgba++ = static_cast(B); + *rgba = vpRGBa::alpha_default; + rgba = rgba + ((4 * width) - 15); + + R = Y8 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y8 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y8 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgba++ = static_cast(R); + *rgba++ = static_cast(G); + *rgba++ = static_cast(B); + *rgba++ = vpRGBa::alpha_default; + + //--- + R = Y9 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y9 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y9 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgba++ = static_cast(R); + *rgba++ = static_cast(G); + *rgba++ = static_cast(B); + *rgba++ = vpRGBa::alpha_default; + + //--- + R = Y10 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y10 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y10 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgba++ = static_cast(R); + *rgba++ = static_cast(G); + *rgba++ = static_cast(B); + *rgba++ = vpRGBa::alpha_default; + + //--- + R = Y11 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y11 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y11 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgba++ = static_cast(R); + *rgba++ = static_cast(G); + *rgba++ = static_cast(B); + *rgba = vpRGBa::alpha_default; + rgba = rgba + ((4 * width) - 15); + + R = Y12 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y12 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y12 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgba++ = static_cast(R); + *rgba++ = static_cast(G); + *rgba++ = static_cast(B); + *rgba++ = vpRGBa::alpha_default; + + //--- + R = Y13 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y13 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y13 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgba++ = static_cast(R); + *rgba++ = static_cast(G); + *rgba++ = static_cast(B); + *rgba++ = vpRGBa::alpha_default; + + //--- + R = Y14 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y14 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y14 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgba++ = static_cast(R); + *rgba++ = static_cast(G); + *rgba++ = static_cast(B); + *rgba++ = vpRGBa::alpha_default; + + //--- + R = Y15 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y15 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y15 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgba++ = static_cast(R); + *rgba++ = static_cast(G); + *rgba++ = static_cast(B); + *rgba = vpRGBa::alpha_default; + rgba = (rgba - (12 * width)) + 1; + } + yuv += 3 * width; + rgba += 12 * width; + } +} + +/*! + Convert YV 1:2 [Y(NxM), V(N/4xM/4), U(N/4xM/4)] image into RGB image. + + \param[in] yuv : Pointer to the bitmap containing the YV 1:2 data. + \param[out] rgb : Pointer to the 24-bits RGB bitmap that should be allocated with a size of width * height * 3. + \param[in] width, height : Image size. +*/ +void vpImageConvert::YVU9ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int height, unsigned int width) +{ + // std::cout << "call optimized ConvertYVU9ToRGB()" << std::endl; + int U, V, R, G, B, V2, U5, UV; + int Y0, Y1, Y2, Y3, Y4, Y5, Y6, Y7, Y8, Y9, Y10, Y11, Y12, Y13, Y14, Y15; + unsigned int size = width * height; + unsigned char *iV = yuv + size; + unsigned char *iU = yuv + ((17 * size) / 16); + const unsigned int quarterHeight = height / 4, quarterWidth = width / 4; + for (unsigned int i = 0; i < quarterHeight; ++i) { + for (unsigned int j = 0; j < quarterWidth; ++j) { + U = static_cast((*iU++ - 128) * 0.354); + U5 = 5 * U; + V = static_cast((*iV++ - 128) * 0.707); + V2 = 2 * V; + UV = -U - V; + Y0 = *yuv++; + Y1 = *yuv++; + Y2 = *yuv++; + Y3 = *yuv; + yuv = yuv + (width - 3); + Y4 = *yuv++; + Y5 = *yuv++; + Y6 = *yuv++; + Y7 = *yuv; + yuv = yuv + (width - 3); + Y8 = *yuv++; + Y9 = *yuv++; + Y10 = *yuv++; + Y11 = *yuv; + yuv = yuv + (width - 3); + Y12 = *yuv++; + Y13 = *yuv++; + Y14 = *yuv++; + Y15 = *yuv; + yuv = (yuv - (3 * width)) + 1; + + // Original equations + // R = Y + 1.402 V + // G = Y - 0.344 U - 0.714 V + // B = Y + 1.772 U + R = Y0 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y0 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y0 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgb++ = static_cast(R); + *rgb++ = static_cast(G); + *rgb++ = static_cast(B); + + //--- + R = Y1 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y1 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y1 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgb++ = static_cast(R); + *rgb++ = static_cast(G); + *rgb++ = static_cast(B); + + //--- + R = Y2 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y2 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y2 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgb++ = static_cast(R); + *rgb++ = static_cast(G); + *rgb++ = static_cast(B); + + //--- + R = Y3 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y3 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y3 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgb++ = static_cast(R); + *rgb++ = static_cast(G); + *rgb = static_cast(B); + rgb = rgb + ((3 * width) - 11); + + R = Y4 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y4 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y4 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgb++ = static_cast(R); + *rgb++ = static_cast(G); + *rgb++ = static_cast(B); + + //--- + R = Y5 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y5 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y5 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgb++ = static_cast(R); + *rgb++ = static_cast(G); + *rgb++ = static_cast(B); + + //--- + R = Y6 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y6 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y6 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgb++ = static_cast(R); + *rgb++ = static_cast(G); + *rgb++ = static_cast(B); + + //--- + R = Y7 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y7 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y7 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgb++ = static_cast(R); + *rgb++ = static_cast(G); + *rgb = static_cast(B); + rgb = rgb + ((3 * width) - 11); + + R = Y8 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y8 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y8 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgb++ = static_cast(R); + *rgb++ = static_cast(G); + *rgb++ = static_cast(B); + + //--- + R = Y9 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y9 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y9 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgb++ = static_cast(R); + *rgb++ = static_cast(G); + *rgb++ = static_cast(B); + + //--- + R = Y10 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y10 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y10 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgb++ = static_cast(R); + *rgb++ = static_cast(G); + *rgb++ = static_cast(B); + + //--- + R = Y11 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y11 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y11 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgb++ = static_cast(R); + *rgb++ = static_cast(G); + *rgb = static_cast(B); + rgb = rgb + 3 * width - 11; + + R = Y12 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y12 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y12 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgb++ = static_cast(R); + *rgb++ = static_cast(G); + *rgb++ = static_cast(B); + + //--- + R = Y13 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y13 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y13 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgb++ = static_cast(R); + *rgb++ = static_cast(G); + *rgb++ = static_cast(B); + + //--- + R = Y14 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y14 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y14 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgb++ = static_cast(R); + *rgb++ = static_cast(G); + *rgb++ = static_cast(B); + + //--- + R = Y15 + V2; + if ((R >> 8) > 0) { + R = 255; + } + else if (R < 0) { + R = 0; + } + + G = Y15 + UV; + if ((G >> 8) > 0) { + G = 255; + } + else if (G < 0) { + G = 0; + } + + B = Y15 + U5; + if ((B >> 8) > 0) { + B = 255; + } + else if (B < 0) { + B = 0; + } + + *rgb++ = static_cast(R); + *rgb++ = static_cast(G); + *rgb++ = static_cast(B); + rgb = (rgb - (9 * width)) + 1; + } + yuv += 3 * width; + rgb += 9 * width; + } +} From 5845ea2a6a9fc84a19af7ac38e21c664d47a13f5 Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Mon, 8 Apr 2024 10:03:12 +0200 Subject: [PATCH 06/17] Satisfy quality rules --- modules/tracker/me/src/moving-edges/vpMe.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/tracker/me/src/moving-edges/vpMe.cpp b/modules/tracker/me/src/moving-edges/vpMe.cpp index 990293c697..d89476ca58 100644 --- a/modules/tracker/me/src/moving-edges/vpMe.cpp +++ b/modules/tracker/me/src/moving-edges/vpMe.cpp @@ -140,7 +140,7 @@ static bool clipping(vpPoint2Dt A, vpPoint2Dt B, double Xmin, double Ymin, doubl vpPoint2Dt P[nbP]; P[0] = A; P[1] = B; - int code_P[nbP], // codes de P[n] + unsigned int code_P[nbP], // codes de P[n] i, bit_i, // i -> (0000100...) n; From 5faa80bc7d950bfb6f629e21d1a710eb08766a89 Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Mon, 8 Apr 2024 10:04:18 +0200 Subject: [PATCH 07/17] Revert changes --- .../testColorConversion.cpp | 83 ------------------- 1 file changed, 83 deletions(-) diff --git a/modules/core/test/image-with-dataset/testColorConversion.cpp b/modules/core/test/image-with-dataset/testColorConversion.cpp index 87d18d3354..04ff6453d8 100644 --- a/modules/core/test/image-with-dataset/testColorConversion.cpp +++ b/modules/core/test/image-with-dataset/testColorConversion.cpp @@ -1451,89 +1451,6 @@ TEST_CASE("RGB to HSV conversion", "[image_conversion]") } } -// Inspired from https://stackoverflow.com/questions/17892346/how-to-convert-rgb-yuv-rgb-both-ways -void YUVToRGB(double Y, double U, double V, double &R, double &G, double &B) -{ - Y -= 16; - U -= 128; - V -= 128; - R = 1.164 * Y + 1.596 * V; - G = 1.164 * Y - 0.391 * U - 0.813 * V; - B = 1.164 * Y + 2.018 * U; - - R = R < 0. ? 0. : R; - G = G < 0. ? 0. : G; - B = B < 0. ? 0. : B; - R = R > 255. ? 255. : R; - G = G > 255. ? 255. : G; - B = B > 255. ? 255. : B; -} - -void RGBToYUV(const double R, const double G, const double B, double &Y, double &U, double &V) -{ - Y = 0.257 * R + 0.504 * G + 0.098 * B + 16; - U = -0.148 * R - 0.291 * G + 0.439 * B + 128; - V = 0.439 * R - 0.368 * G - 0.071 * B + 128; - - Y = Y < 0. ? 0. : Y; - U = U < 0. ? 0. : U; - V = V < 0. ? 0. : V; - Y = Y > 255. ? 255. : Y; - U = U > 255. ? 255. : U; - V = V > 255. ? 255. : V; -} - -TEST_CASE("YUV to RGB conversion", "[image_conversion]") -{ - std::vector< std::vector > rgb_truth; - rgb_truth.push_back({ 0, 0, 0 }); - rgb_truth.push_back({ 255, 255, 255 }); - rgb_truth.push_back({ 255, 0, 0 }); - rgb_truth.push_back({ 0, 255, 0 }); - rgb_truth.push_back({ 0, 0, 255 }); - rgb_truth.push_back({ 255, 255, 0 }); - rgb_truth.push_back({ 0, 255, 255 }); - rgb_truth.push_back({ 255, 0, 255 }); - rgb_truth.push_back({ 128, 128, 128 }); - rgb_truth.push_back({ 128, 128, 0 }); - rgb_truth.push_back({ 128, 0, 0 }); - rgb_truth.push_back({ 0, 128, 0 }); - rgb_truth.push_back({ 0, 128, 128 }); - rgb_truth.push_back({ 0, 0, 128 }); - rgb_truth.push_back({ 128, 0, 128 }); - - size_t size = rgb_truth.size(); - - std::vector< std::vector > yuv_truth; - for (size_t i = 0; i < size; ++i) { - double y, u, v; - double r = static_cast(rgb_truth[i][0]); - double g = static_cast(rgb_truth[i][1]); - double b = static_cast(rgb_truth[i][2]); - RGBToYUV(r, g, b, y, u, v); - yuv_truth.push_back({ y, u, v }); - } - SECTION("RGB -> YUV") - { - for (size_t i = 0; i < size; ++i) { - unsigned char r, g, b; - unsigned char y = static_cast(yuv_truth[i][0]); - unsigned char u = static_cast(yuv_truth[i][1]); - unsigned char v = static_cast(yuv_truth[i][2]); - - std::cout << "rgb truth (uchar ): " << (int)rgb_truth[i][0] << " " << (int)rgb_truth[i][1] << " " << (int)rgb_truth[i][2] << std::endl; - std::cout << "yuv truth (double): " << yuv_truth[i][0] << " " << yuv_truth[i][1] << " " << yuv_truth[i][2] << std::endl; - std::cout << "yuv truth (uchar ): " << (int)y << " " << (int)u << " " << (int)v << std::endl; - vpImageConvert::YUVToRGB(y, u, v, r, g, b); - std::cout << "rgb (uchar ): " << (int)r << " " << (int)g << " " << (int)b << std::endl; - double r_truth, g_truth, b_truth; - YUVToRGB(yuv_truth[i][0], yuv_truth[i][1], yuv_truth[i][2], r_truth, g_truth, b_truth); - std::cout << "rgb (double): " << r_truth << " " << g_truth << " " << b_truth << std::endl; - - } - } -} - int main(int argc, char *argv[]) { Catch::Session session; // There must be exactly one instance From 0094eec7a4efc102a962b2a15dfbca7c643ae583 Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Mon, 8 Apr 2024 15:55:50 +0200 Subject: [PATCH 08/17] Fix firewire 1394 grabber example --- example/device/framegrabber/grab1394Two.cpp | 15 +++---- .../framegrabber/1394/vp1394TwoGrabber.cpp | 40 +++++++++---------- 2 files changed, 28 insertions(+), 27 deletions(-) diff --git a/example/device/framegrabber/grab1394Two.cpp b/example/device/framegrabber/grab1394Two.cpp index 4e0634dc52..7fd82b026d 100644 --- a/example/device/framegrabber/grab1394Two.cpp +++ b/example/device/framegrabber/grab1394Two.cpp @@ -320,7 +320,7 @@ bool read_options(int argc, const char **argv, bool &multi, unsigned int &camera case '?': usage(argv[0], nullptr, camera, nframes, opath, roi_left, roi_top, roi_width, roi_height, ringbuffersize, panControl); - exit(0); + return false; break; } } @@ -376,10 +376,9 @@ int main(int argc, const char **argv) if (read_options(argc, argv, multi, camera, nframes, verbose_info, verbose_settings, videomode_is_set, videomode, framerate_is_set, framerate, colorcoding_is_set, colorcoding, ringbuffersize_is_set, ringbuffersize, display, save, opath, roi_left, roi_top, roi_width, roi_height, reset, panControl, - panControl_is_set)) { + panControl_is_set) == false) { return EXIT_FAILURE; } - // Create a grabber vp1394TwoGrabber g(reset); @@ -465,11 +464,13 @@ int main(int argc, const char **argv) for (it_lmode = lmode.begin(); it_lmode != lmode.end(); ++it_lmode) { // Parse the list of supported modes vp1394TwoGrabber::vp1394TwoVideoModeType supmode = *it_lmode; + std::stringstream ss; + ss << (int)supmode; if (curmode == supmode) - std::cout << " * " << vp1394TwoGrabber::videoMode2string(supmode) << " (-v " << (int)supmode << ")" + std::cout << " * " << vp1394TwoGrabber::videoMode2string(supmode) << " (-v " << ss.str() << ")" << std::endl; else - std::cout << " " << vp1394TwoGrabber::videoMode2string(supmode) << " (-v " << (int)supmode << ")" + std::cout << " " << vp1394TwoGrabber::videoMode2string(supmode) << " (-v " << ss.str() << ")" << std::endl; if (g.isVideoModeFormat7(supmode)) { @@ -489,7 +490,7 @@ int main(int argc, const char **argv) } else { - // Parse the list of supported framerates for a supported mode + // Parse the list of supported framerates for a supported mode g.getFramerateSupported(supmode, lfps); for (it_lfps = lfps.begin(); it_lfps != lfps.end(); ++it_lfps) { vp1394TwoGrabber::vp1394TwoFramerateType supfps = *it_lfps; @@ -520,7 +521,7 @@ int main(int argc, const char **argv) g.setVideoMode(videomode); } else { - // get The actual video mode + // get The actual video mode g.setCamera(camera); g.getVideoMode(videomode); } diff --git a/modules/sensor/src/framegrabber/1394/vp1394TwoGrabber.cpp b/modules/sensor/src/framegrabber/1394/vp1394TwoGrabber.cpp index 732c012c28..fec67e35d9 100644 --- a/modules/sensor/src/framegrabber/1394/vp1394TwoGrabber.cpp +++ b/modules/sensor/src/framegrabber/1394/vp1394TwoGrabber.cpp @@ -81,11 +81,11 @@ const char *vp1394TwoGrabber::strColorCoding[DC1394_COLOR_CODING_NUM] = { - reset the bus attached to the first camera found on the bus. Current camera settings can be changed using setCamera() to select the -active camera on the bus and than setVideoMode() or setFramerate() to fix the -active camera settings. The list of supported video modes and framerates is + active camera on the bus and than setVideoMode() or setFramerate() to fix the + active camera settings. The list of supported video modes and framerates is available using respectively getVideoModeSupported() and getFramerateSupported(). To change the ring buffer size use -setRingBufferSize(). + setRingBufferSize(). \param reset : If "true", reset the bus attached to the first camera found. Bus reset may help to make firewire working if the @@ -95,22 +95,22 @@ setRingBufferSize(). Format7 with a transmission speed set to 800Mbps in 1394b mode. \code -#include -#include + #include + #include -int main() -{ -#if defined(VISP_HAVE_DC1394) - vpImage I; - vp1394TwoGrabber g(false); // Don't reset the bus - g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_0 ); - g.setColorCoding(vp1394TwoGrabber::vpCOLOR_CODING_MONO8); - g.setAutoShutter(1600*20-1, 1600*20); // Set shutter min and max values - g.setIsoTransmissionSpeed(vp1394TwoGrabber::vpISO_SPEED_800); // 1394b - while(1) - g.acquire(I); -#endif -} + int main() + { + #if defined(VISP_HAVE_DC1394) + vpImage I; + vp1394TwoGrabber g(false); // Don't reset the bus + g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_0 ); + g.setColorCoding(vp1394TwoGrabber::vpCOLOR_CODING_MONO8); + g.setAutoShutter(1600*20-1, 1600*20); // Set shutter min and max values + g.setIsoTransmissionSpeed(vp1394TwoGrabber::vpISO_SPEED_800); // 1394b + while(1) + g.acquire(I); + #endif + } \endcode \sa setCamera(), setVideoMode(), setFramerate() @@ -511,7 +511,6 @@ void vp1394TwoGrabber::getVideoMode(vp1394TwoVideoModeType &videomode) Query the available active camera video modes. - \param videomodes : The list of supported camera video modes. \return The number of supported camera modes, 0 if an error occurs. @@ -587,8 +586,9 @@ bool vp1394TwoGrabber::isVideoModeSupported(vp1394TwoVideoModeType videomode) // parse the video modes to check with the desired for (unsigned i = 0; i < _videomodes.num; i++) { - if ((vp1394TwoVideoModeType)_videomodes.modes[i] == videomode) + if ((vp1394TwoVideoModeType)_videomodes.modes[i] == videomode) { return true; + } } return false; } From 9eb6a5de53d5f3d4e8be9a53c97192bbe9717ff3 Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Mon, 8 Apr 2024 17:17:02 +0200 Subject: [PATCH 09/17] Relax testGenericTracker-edge-scanline --- .../mbt/test/generic-with-dataset/testGenericTracker.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/tracker/mbt/test/generic-with-dataset/testGenericTracker.cpp b/modules/tracker/mbt/test/generic-with-dataset/testGenericTracker.cpp index 69a729b482..c44e982366 100644 --- a/modules/tracker/mbt/test/generic-with-dataset/testGenericTracker.cpp +++ b/modules/tracker/mbt/test/generic-with-dataset/testGenericTracker.cpp @@ -382,7 +382,7 @@ bool run(const std::string &input_directory, bool opt_click_allowed, bool opt_di #endif #else map_thresh[vpMbGenericTracker::EDGE_TRACKER] = - useScanline ? std::pair(0.01, 2.5) : std::pair(0.009, 4.0); + useScanline ? std::pair(0.015, 2.5) : std::pair(0.009, 4.0); #if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO) map_thresh[vpMbGenericTracker::KLT_TRACKER] = useScanline ? std::pair(0.006, 1.7) : std::pair(0.005, 1.4); From 69e95f028bd2e68c8a5158b40f250493f71a9fbb Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Mon, 8 Apr 2024 18:37:03 +0200 Subject: [PATCH 10/17] Fix 1394 example around -f -g and -v options usage --- example/device/framegrabber/grab1394Two.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/example/device/framegrabber/grab1394Two.cpp b/example/device/framegrabber/grab1394Two.cpp index 7fd82b026d..77018c3c1e 100644 --- a/example/device/framegrabber/grab1394Two.cpp +++ b/example/device/framegrabber/grab1394Two.cpp @@ -480,11 +480,13 @@ int main(int argc, const char **argv) for (it_lcoding = lcoding.begin(); it_lcoding != lcoding.end(); ++it_lcoding) { vp1394TwoGrabber::vp1394TwoColorCodingType supcoding; supcoding = *it_lcoding; + std::stringstream ss; + ss << (int)supcoding; if ((curmode == supmode) && (supcoding == curcoding)) - std::cout << " * " << vp1394TwoGrabber::colorCoding2string(supcoding) << " (-g " << (int)supcoding + std::cout << " * " << vp1394TwoGrabber::colorCoding2string(supcoding) << " (-g " << ss.str() << ")" << std::endl; else - std::cout << " " << vp1394TwoGrabber::colorCoding2string(supcoding) << " (-g " << (int)supcoding + std::cout << " " << vp1394TwoGrabber::colorCoding2string(supcoding) << " (-g " << ss.str() << ")" << std::endl; } } @@ -494,11 +496,13 @@ int main(int argc, const char **argv) g.getFramerateSupported(supmode, lfps); for (it_lfps = lfps.begin(); it_lfps != lfps.end(); ++it_lfps) { vp1394TwoGrabber::vp1394TwoFramerateType supfps = *it_lfps; + std::stringstream ss; + ss << (int)supfps; if ((curmode == supmode) && (supfps == curfps)) - std::cout << " * " << vp1394TwoGrabber::framerate2string(supfps) << " (-f " << (int)supfps << ")" + std::cout << " * " << vp1394TwoGrabber::framerate2string(supfps) << " (-f " << ss.str() << ")" << std::endl; else - std::cout << " " << vp1394TwoGrabber::framerate2string(supfps) << " (-f " << (int)supfps << ")" + std::cout << " " << vp1394TwoGrabber::framerate2string(supfps) << " (-f " << ss.str() << ")" << std::endl; } } From 6ebc865cb0abc67c8c69372b1f32cdba622c029b Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Mon, 8 Apr 2024 18:37:10 +0200 Subject: [PATCH 11/17] Remove bitwise operator on signed var to satisfy misra c++ quality rule --- modules/core/src/image/vpImageConvert_yuv.cpp | 1471 +++-------------- 1 file changed, 207 insertions(+), 1264 deletions(-) diff --git a/modules/core/src/image/vpImageConvert_yuv.cpp b/modules/core/src/image/vpImageConvert_yuv.cpp index 2291700634..b2ac34c5ac 100644 --- a/modules/core/src/image/vpImageConvert_yuv.cpp +++ b/modules/core/src/image/vpImageConvert_yuv.cpp @@ -30,8 +30,7 @@ * * Description: * Convert image types. - * -*****************************************************************************/ + */ /*! \file vpImageConvert_yuv.cpp @@ -73,21 +72,23 @@ void vpImageConvert::YUYVToRGBa(unsigned char *yuyv, unsigned char *rgba, unsign s = yuyv; d = rgba; while (h--) { - int c = w >> 1; + int c = w / 2; while (c--) { y1 = *s++; - cb = ((*s - 128) * 454) >> 8; + cb = ((*s - 128) * 454) / 256; cg = (*s++ - 128) * 88; y2 = *s++; - cr = ((*s - 128) * 359) >> 8; - cg = (cg + ((*s++ - 128) * 183)) >> 8; + cr = ((*s - 128) * 359) / 256; + cg = (cg + ((*s++ - 128) * 183)) / 256; r = y1 + cr; b = y1 + cb; g = y1 - cg; - vpSAT(r) vpSAT(g) vpSAT(b) + vpSAT(r); + vpSAT(g); + vpSAT(b); - *d++ = static_cast(r); + *d++ = static_cast(r); *d++ = static_cast(g); *d++ = static_cast(b); *d++ = vpRGBa::alpha_default; @@ -95,9 +96,11 @@ void vpImageConvert::YUYVToRGBa(unsigned char *yuyv, unsigned char *rgba, unsign r = y2 + cr; b = y2 + cb; g = y2 - cg; - vpSAT(r) vpSAT(g) vpSAT(b) + vpSAT(r); + vpSAT(g); + vpSAT(b); - *d++ = static_cast(r); + *d++ = static_cast(r); *d++ = static_cast(g); *d++ = static_cast(b); *d++ = vpRGBa::alpha_default; @@ -127,30 +130,30 @@ void vpImageConvert::YUYVToRGB(unsigned char *yuyv, unsigned char *rgb, unsigned s = yuyv; d = rgb; while (h--) { - int c = w >> 1; + int c = w / 2; while (c--) { y1 = *s++; - cb = ((*s - 128) * 454) >> 8; + cb = ((*s - 128) * 454) / 256; cg = (*s++ - 128) * 88; y2 = *s++; - cr = ((*s - 128) * 359) >> 8; - cg = (cg + ((*s++ - 128) * 183)) >> 8; + cr = ((*s - 128) * 359) / 256; + cg = (cg + ((*s++ - 128) * 183)) / 256; r = y1 + cr; b = y1 + cb; g = y1 - cg; - vpSAT(r) vpSAT(g) vpSAT(b) + vpSAT(r); vpSAT(g); vpSAT(b); - *d++ = static_cast(r); + *d++ = static_cast(r); *d++ = static_cast(g); *d++ = static_cast(b); r = y2 + cr; b = y2 + cb; g = y2 - cg; - vpSAT(r) vpSAT(g) vpSAT(b) + vpSAT(r); vpSAT(g); vpSAT(b); - *d++ = static_cast(r); + *d++ = static_cast(r); *d++ = static_cast(g); *d++ = static_cast(b); } @@ -189,8 +192,6 @@ void vpImageConvert::YUYVToGrey(unsigned char *yuyv, unsigned char *grey, unsign */ void vpImageConvert::YUV411ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int size) { -#if 1 - // std::cout << "call optimized ConvertYUV411ToRGBa()" << std::endl; for (unsigned int i = size / 4; i; --i) { int U = static_cast((*yuv++ - 128) * 0.354); int U5 = 5 * U; @@ -207,28 +208,13 @@ void vpImageConvert::YUV411ToRGBa(unsigned char *yuv, unsigned char *rgba, unsig // G = Y - 0.344 U - 0.714 V // B = Y + 1.772 U int R = Y0 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); int G = Y0 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); int B = Y0 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgba++ = static_cast(R); *rgba++ = static_cast(G); @@ -237,28 +223,13 @@ void vpImageConvert::YUV411ToRGBa(unsigned char *yuv, unsigned char *rgba, unsig //--- R = Y1 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y1 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y1 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgba++ = static_cast(R); *rgba++ = static_cast(G); @@ -267,28 +238,13 @@ void vpImageConvert::YUV411ToRGBa(unsigned char *yuv, unsigned char *rgba, unsig //--- R = Y2 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y2 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y2 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgba++ = static_cast(R); *rgba++ = static_cast(G); @@ -297,72 +253,19 @@ void vpImageConvert::YUV411ToRGBa(unsigned char *yuv, unsigned char *rgba, unsig //--- R = Y3 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y3 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y3 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgba++ = static_cast(R); *rgba++ = static_cast(G); *rgba++ = static_cast(B); *rgba++ = vpRGBa::alpha_default; } -#else - // tres tres lent .... - unsigned int i = 0, j = 0; - unsigned char r, g, b; - const unsigned int iterLimit = (numpixels * 3) / 2; - while (j < iterLimit) { - - YUVToRGB(yuv[j + 1], yuv[j], yuv[j + 3], r, g, b); - rgba[i] = r; - rgba[i + 1] = g; - rgba[i + 2] = b; - rgba[i + 3] = vpRGBa::alpha_default; - i += 4; - - YUVToRGB(yuv[j + 2], yuv[j], yuv[j + 3], r, g, b); - rgba[i] = r; - rgba[i + 1] = g; - rgba[i + 2] = b; - rgba[i + 3] = vpRGBa::alpha_default; - i += 4; - - YUVToRGB(yuv[j + 4], yuv[j], yuv[j + 3], r, g, b); - rgba[i] = r; - rgba[i + 1] = g; - rgba[i + 2] = b; - rgba[i + 3] = vpRGBa::alpha_default; - i += 4; - - YUVToRGB(yuv[j + 5], yuv[j], yuv[j + 3], r, g, b); - rgba[i] = r; - rgba[i + 1] = g; - rgba[i + 2] = b; - rgba[i + 3] = vpRGBa::alpha_default; - i += 4; - - j += 6; - } -#endif } /*! @@ -379,9 +282,6 @@ void vpImageConvert::YUV411ToRGBa(unsigned char *yuv, unsigned char *rgba, unsig */ void vpImageConvert::YUV422ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int size) { - -#if 1 - // std::cout << "call optimized convertYUV422ToRGBa()" << std::endl; for (unsigned int i = size / 2; i; --i) { int U = static_cast((*yuv++ - 128) * 0.354); int U5 = 5 * U; @@ -393,28 +293,13 @@ void vpImageConvert::YUV422ToRGBa(unsigned char *yuv, unsigned char *rgba, unsig //--- int R = Y0 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); int G = Y0 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); int B = Y0 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgba++ = static_cast(R); *rgba++ = static_cast(G); @@ -423,58 +308,19 @@ void vpImageConvert::YUV422ToRGBa(unsigned char *yuv, unsigned char *rgba, unsig //--- R = Y1 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y1 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y1 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgba++ = static_cast(R); *rgba++ = static_cast(G); *rgba++ = static_cast(B); *rgba++ = vpRGBa::alpha_default; } - -#else - // tres tres lent .... - unsigned int i = 0, j = 0; - unsigned char r, g, b; - const unsigned int doubleSize = size * 2; - while (j < doubleSize) { - - YUVToRGB(yuv[j + 1], yuv[j], yuv[j + 2], r, g, b); - rgba[i] = r; - rgba[i + 1] = g; - rgba[i + 2] = b; - rgba[i + 3] = vpRGBa::alpha_default; - i += 4; - - YUVToRGB(yuv[j + 3], yuv[j], yuv[j + 2], r, g, b); - rgba[i] = r; - rgba[i + 1] = g; - rgba[i + 2] = b; - rgba[i + 3] = vpRGBa::alpha_default; - i += 4; - j += 4; - } -#endif } /*! @@ -513,8 +359,6 @@ void vpImageConvert::YUV411ToGrey(unsigned char *yuv, unsigned char *grey, unsig */ void vpImageConvert::YUV422ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int size) { -#if 1 - // std::cout << "call optimized convertYUV422ToRGB()" << std::endl; for (unsigned int i = size / 2; i; --i) { int U = static_cast((*yuv++ - 128) * 0.354); int U5 = 5 * U; @@ -526,28 +370,13 @@ void vpImageConvert::YUV422ToRGB(unsigned char *yuv, unsigned char *rgb, unsigne //--- int R = Y0 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); int G = Y0 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); int B = Y0 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgb++ = static_cast(R); *rgb++ = static_cast(G); @@ -555,55 +384,18 @@ void vpImageConvert::YUV422ToRGB(unsigned char *yuv, unsigned char *rgb, unsigne //--- R = Y1 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y1 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y1 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgb++ = static_cast(R); *rgb++ = static_cast(G); *rgb++ = static_cast(B); } - -#else - // tres tres lent .... - unsigned int i = 0, j = 0; - unsigned char r, g, b; - const unsigned int doubleSize = size * 2; - while (j < doubleSize) { - - YUVToRGB(yuv[j + 1], yuv[j], yuv[j + 2], r, g, b); - rgb[i] = r; - rgb[i + 1] = g; - rgb[i + 2] = b; - i += 3; - - YUVToRGB(yuv[j + 3], yuv[j], yuv[j + 2], r, g, b); - rgb[i] = r; - rgb[i + 1] = g; - rgb[i + 2] = b; - i += 3; - j += 4; - } -#endif } /*! @@ -638,8 +430,6 @@ void vpImageConvert::YUV422ToGrey(unsigned char *yuv, unsigned char *grey, unsig */ void vpImageConvert::YUV411ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int size) { -#if 1 - // std::cout << "call optimized ConvertYUV411ToRGB()" << std::endl; for (unsigned int i = size / 4; i; --i) { int U = static_cast((*yuv++ - 128) * 0.354); int U5 = 5 * U; @@ -656,28 +446,13 @@ void vpImageConvert::YUV411ToRGB(unsigned char *yuv, unsigned char *rgb, unsigne // G = Y - 0.344 U - 0.714 V // B = Y + 1.772 U int R = Y0 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); int G = Y0 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); int B = Y0 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgb++ = static_cast(R); *rgb++ = static_cast(G); @@ -685,28 +460,13 @@ void vpImageConvert::YUV411ToRGB(unsigned char *yuv, unsigned char *rgb, unsigne //--- R = Y1 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y1 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y1 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgb++ = static_cast(R); *rgb++ = static_cast(G); @@ -714,28 +474,13 @@ void vpImageConvert::YUV411ToRGB(unsigned char *yuv, unsigned char *rgb, unsigne //--- R = Y2 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y2 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y2 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgb++ = static_cast(R); *rgb++ = static_cast(G); @@ -743,69 +488,18 @@ void vpImageConvert::YUV411ToRGB(unsigned char *yuv, unsigned char *rgb, unsigne //--- R = Y3 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y3 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y3 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgb++ = static_cast(R); *rgb++ = static_cast(G); *rgb++ = static_cast(B); } -#else - // tres tres lent .... - - unsigned int i = 0, j = 0; - unsigned char r, g, b; - - const unsigned int iterLimit = (size * 3) / 2; - while (j < iterLimit) { - YUVToRGB(yuv[j + 1], yuv[j], yuv[j + 3], r, g, b); - rgb[i] = r; - rgb[i + 1] = g; - rgb[i + 2] = b; - i += 3; - - YUVToRGB(yuv[j + 2], yuv[j], yuv[j + 3], r, g, b); - rgb[i] = r; - rgb[i + 1] = g; - rgb[i + 2] = b; - i += 3; - - YUVToRGB(yuv[j + 4], yuv[j], yuv[j + 3], r, g, b); - rgb[i] = r; - rgb[i + 1] = g; - rgb[i + 2] = b; - i += 3; - - YUVToRGB(yuv[j + 5], yuv[j], yuv[j + 3], r, g, b); - rgb[i] = r; - rgb[i + 1] = g; - rgb[i + 2] = b; - i += 3; - // TRACE("r= %d g=%d b=%d", r, g, b); - - j += 6; - } -#endif } /*! @@ -846,28 +540,13 @@ void vpImageConvert::YUV420ToRGBa(unsigned char *yuv, unsigned char *rgba, unsig // G = Y - 0.344 U - 0.714 V // B = Y + 1.772 U R = Y0 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y0 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y0 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgba++ = static_cast(R); *rgba++ = static_cast(G); @@ -876,28 +555,13 @@ void vpImageConvert::YUV420ToRGBa(unsigned char *yuv, unsigned char *rgba, unsig //--- R = Y1 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y1 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y1 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgba++ = static_cast(R); *rgba++ = static_cast(G); @@ -907,28 +571,13 @@ void vpImageConvert::YUV420ToRGBa(unsigned char *yuv, unsigned char *rgba, unsig //--- R = Y2 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y2 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y2 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgba++ = static_cast(R); *rgba++ = static_cast(G); @@ -937,28 +586,13 @@ void vpImageConvert::YUV420ToRGBa(unsigned char *yuv, unsigned char *rgba, unsig //--- R = Y3 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y3 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y3 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgba++ = static_cast(R); *rgba++ = static_cast(G); @@ -981,7 +615,6 @@ void vpImageConvert::YUV420ToRGBa(unsigned char *yuv, unsigned char *rgba, unsig */ void vpImageConvert::YUV420ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int width, unsigned int height) { - // std::cout << "call optimized ConvertYUV420ToRGB()" << std::endl; int U, V, R, G, B, V2, U5, UV; int Y0, Y1, Y2, Y3; unsigned int size = width * height; @@ -1007,28 +640,13 @@ void vpImageConvert::YUV420ToRGB(unsigned char *yuv, unsigned char *rgb, unsigne // G = Y - 0.344 U - 0.714 V // B = Y + 1.772 U R = Y0 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y0 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y0 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgb++ = static_cast(R); *rgb++ = static_cast(G); @@ -1036,28 +654,13 @@ void vpImageConvert::YUV420ToRGB(unsigned char *yuv, unsigned char *rgb, unsigne //--- R = Y1 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y1 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y1 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgb++ = static_cast(R); *rgb++ = static_cast(G); @@ -1066,28 +669,13 @@ void vpImageConvert::YUV420ToRGB(unsigned char *yuv, unsigned char *rgb, unsigne //--- R = Y2 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y2 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y2 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgb++ = static_cast(R); *rgb++ = static_cast(G); @@ -1095,28 +683,13 @@ void vpImageConvert::YUV420ToRGB(unsigned char *yuv, unsigned char *rgb, unsigne //--- R = Y3 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y3 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y3 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgb++ = static_cast(R); *rgb++ = static_cast(G); @@ -1166,28 +739,13 @@ void vpImageConvert::YUV444ToRGBa(unsigned char *yuv, unsigned char *rgba, unsig // G = Y - 0.344 U - 0.714 V // B = Y + 1.772 U int R = Y + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); int G = Y + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); int B = Y + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgba++ = static_cast(R); *rgba++ = static_cast(G); @@ -1218,28 +776,13 @@ void vpImageConvert::YUV444ToRGB(unsigned char *yuv, unsigned char *rgb, unsigne // G = Y - 0.344 U - 0.714 V // B = Y + 1.772 U int R = Y + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); int G = Y + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); int B = Y + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgb++ = static_cast(R); *rgb++ = static_cast(G); @@ -1301,28 +844,13 @@ void vpImageConvert::YV12ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne // G = Y - 0.344 U - 0.714 V // B = Y + 1.772 U R = Y0 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y0 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y0 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgba++ = static_cast(R); *rgba++ = static_cast(G); @@ -1331,28 +859,13 @@ void vpImageConvert::YV12ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne //--- R = Y1 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y1 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y1 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgba++ = static_cast(R); *rgba++ = static_cast(G); @@ -1362,28 +875,13 @@ void vpImageConvert::YV12ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne //--- R = Y2 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y2 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y2 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgba++ = static_cast(R); *rgba++ = static_cast(G); @@ -1392,28 +890,13 @@ void vpImageConvert::YV12ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne //--- R = Y3 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y3 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y3 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgba++ = static_cast(R); *rgba++ = static_cast(G); @@ -1462,28 +945,13 @@ void vpImageConvert::YV12ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned // G = Y - 0.344 U - 0.714 V // B = Y + 1.772 U R = Y0 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y0 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y0 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgb++ = static_cast(R); *rgb++ = static_cast(G); @@ -1491,28 +959,13 @@ void vpImageConvert::YV12ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned //--- R = Y1 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y1 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y1 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgb++ = static_cast(R); *rgb++ = static_cast(G); @@ -1521,28 +974,13 @@ void vpImageConvert::YV12ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned //--- R = Y2 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y2 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y2 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgb++ = static_cast(R); *rgb++ = static_cast(G); @@ -1550,28 +988,13 @@ void vpImageConvert::YV12ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned //--- R = Y3 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y3 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y3 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgb++ = static_cast(R); *rgb++ = static_cast(G); @@ -1635,28 +1058,13 @@ void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne // G = Y - 0.344 U - 0.714 V // B = Y + 1.772 U R = Y0 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y0 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y0 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgba++ = static_cast(R); *rgba++ = static_cast(G); @@ -1665,28 +1073,13 @@ void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne //--- R = Y1 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y1 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y1 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgba++ = static_cast(R); *rgba++ = static_cast(G); @@ -1695,28 +1088,13 @@ void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne //--- R = Y2 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y2 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y2 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgba++ = static_cast(R); *rgba++ = static_cast(G); @@ -1725,28 +1103,13 @@ void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne //--- R = Y3 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y3 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y3 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgba++ = static_cast(R); *rgba++ = static_cast(G); @@ -1755,28 +1118,13 @@ void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne rgba = rgba + ((4 * width) - 15); R = Y4 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y4 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y4 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgba++ = static_cast(R); *rgba++ = static_cast(G); @@ -1785,28 +1133,13 @@ void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne //--- R = Y5 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y5 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y5 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgba++ = static_cast(R); *rgba++ = static_cast(G); @@ -1815,28 +1148,13 @@ void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne //--- R = Y6 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y6 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y6 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgba++ = static_cast(R); *rgba++ = static_cast(G); @@ -1845,28 +1163,13 @@ void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne //--- R = Y7 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y7 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y7 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgba++ = static_cast(R); *rgba++ = static_cast(G); @@ -1875,28 +1178,13 @@ void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne rgba = rgba + ((4 * width) - 15); R = Y8 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y8 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y8 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgba++ = static_cast(R); *rgba++ = static_cast(G); @@ -1905,28 +1193,13 @@ void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne //--- R = Y9 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y9 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y9 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgba++ = static_cast(R); *rgba++ = static_cast(G); @@ -1935,28 +1208,13 @@ void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne //--- R = Y10 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y10 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y10 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgba++ = static_cast(R); *rgba++ = static_cast(G); @@ -1965,28 +1223,13 @@ void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne //--- R = Y11 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y11 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y11 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgba++ = static_cast(R); *rgba++ = static_cast(G); @@ -1995,28 +1238,13 @@ void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne rgba = rgba + ((4 * width) - 15); R = Y12 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y12 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y12 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgba++ = static_cast(R); *rgba++ = static_cast(G); @@ -2025,28 +1253,13 @@ void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne //--- R = Y13 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y13 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y13 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgba++ = static_cast(R); *rgba++ = static_cast(G); @@ -2055,28 +1268,13 @@ void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne //--- R = Y14 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y14 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y14 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgba++ = static_cast(R); *rgba++ = static_cast(G); @@ -2085,28 +1283,13 @@ void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne //--- R = Y15 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y15 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y15 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgba++ = static_cast(R); *rgba++ = static_cast(G); @@ -2168,28 +1351,13 @@ void vpImageConvert::YVU9ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned // G = Y - 0.344 U - 0.714 V // B = Y + 1.772 U R = Y0 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y0 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y0 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgb++ = static_cast(R); *rgb++ = static_cast(G); @@ -2197,28 +1365,13 @@ void vpImageConvert::YVU9ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned //--- R = Y1 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y1 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y1 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgb++ = static_cast(R); *rgb++ = static_cast(G); @@ -2226,28 +1379,13 @@ void vpImageConvert::YVU9ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned //--- R = Y2 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y2 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y2 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgb++ = static_cast(R); *rgb++ = static_cast(G); @@ -2255,28 +1393,13 @@ void vpImageConvert::YVU9ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned //--- R = Y3 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y3 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y3 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgb++ = static_cast(R); *rgb++ = static_cast(G); @@ -2284,28 +1407,13 @@ void vpImageConvert::YVU9ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned rgb = rgb + ((3 * width) - 11); R = Y4 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y4 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y4 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgb++ = static_cast(R); *rgb++ = static_cast(G); @@ -2313,28 +1421,13 @@ void vpImageConvert::YVU9ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned //--- R = Y5 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y5 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y5 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgb++ = static_cast(R); *rgb++ = static_cast(G); @@ -2342,28 +1435,13 @@ void vpImageConvert::YVU9ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned //--- R = Y6 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y6 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y6 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgb++ = static_cast(R); *rgb++ = static_cast(G); @@ -2371,28 +1449,13 @@ void vpImageConvert::YVU9ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned //--- R = Y7 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y7 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y7 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgb++ = static_cast(R); *rgb++ = static_cast(G); @@ -2400,28 +1463,13 @@ void vpImageConvert::YVU9ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned rgb = rgb + ((3 * width) - 11); R = Y8 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y8 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y8 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgb++ = static_cast(R); *rgb++ = static_cast(G); @@ -2429,28 +1477,13 @@ void vpImageConvert::YVU9ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned //--- R = Y9 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y9 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y9 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgb++ = static_cast(R); *rgb++ = static_cast(G); @@ -2458,28 +1491,13 @@ void vpImageConvert::YVU9ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned //--- R = Y10 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y10 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y10 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgb++ = static_cast(R); *rgb++ = static_cast(G); @@ -2487,28 +1505,13 @@ void vpImageConvert::YVU9ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned //--- R = Y11 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y11 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y11 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgb++ = static_cast(R); *rgb++ = static_cast(G); @@ -2516,28 +1519,13 @@ void vpImageConvert::YVU9ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned rgb = rgb + 3 * width - 11; R = Y12 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y12 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y12 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgb++ = static_cast(R); *rgb++ = static_cast(G); @@ -2545,28 +1533,13 @@ void vpImageConvert::YVU9ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned //--- R = Y13 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y13 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y13 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgb++ = static_cast(R); *rgb++ = static_cast(G); @@ -2574,28 +1547,13 @@ void vpImageConvert::YVU9ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned //--- R = Y14 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y14 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y14 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgb++ = static_cast(R); *rgb++ = static_cast(G); @@ -2603,28 +1561,13 @@ void vpImageConvert::YVU9ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned //--- R = Y15 + V2; - if ((R >> 8) > 0) { - R = 255; - } - else if (R < 0) { - R = 0; - } + vpSAT(R); G = Y15 + UV; - if ((G >> 8) > 0) { - G = 255; - } - else if (G < 0) { - G = 0; - } + vpSAT(G); B = Y15 + U5; - if ((B >> 8) > 0) { - B = 255; - } - else if (B < 0) { - B = 0; - } + vpSAT(B); *rgb++ = static_cast(R); *rgb++ = static_cast(G); From 179a7b1360611fb8fac9999c081feca4b67986f6 Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Mon, 8 Apr 2024 18:41:29 +0200 Subject: [PATCH 12/17] Relax more testGenericTracker-edge-scanline --- .../mbt/test/generic-with-dataset/testGenericTracker.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/tracker/mbt/test/generic-with-dataset/testGenericTracker.cpp b/modules/tracker/mbt/test/generic-with-dataset/testGenericTracker.cpp index c44e982366..cc551630d4 100644 --- a/modules/tracker/mbt/test/generic-with-dataset/testGenericTracker.cpp +++ b/modules/tracker/mbt/test/generic-with-dataset/testGenericTracker.cpp @@ -382,7 +382,7 @@ bool run(const std::string &input_directory, bool opt_click_allowed, bool opt_di #endif #else map_thresh[vpMbGenericTracker::EDGE_TRACKER] = - useScanline ? std::pair(0.015, 2.5) : std::pair(0.009, 4.0); + useScanline ? std::pair(0.015, 3.0) : std::pair(0.009, 4.0); #if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO) map_thresh[vpMbGenericTracker::KLT_TRACKER] = useScanline ? std::pair(0.006, 1.7) : std::pair(0.005, 1.4); From c63dffa0b91e2435f34c5d7cc7aaff59543865ca Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Tue, 9 Apr 2024 15:53:07 +0200 Subject: [PATCH 13/17] Fix typo --- .../testColorConversion.cpp | 20 +++++++++---------- .../src/pose-estimation/vpPoseRansac.cpp | 2 +- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/modules/core/test/image-with-dataset/testColorConversion.cpp b/modules/core/test/image-with-dataset/testColorConversion.cpp index 04ff6453d8..3ee2ded944 100644 --- a/modules/core/test/image-with-dataset/testColorConversion.cpp +++ b/modules/core/test/image-with-dataset/testColorConversion.cpp @@ -930,7 +930,7 @@ static double computePSNR(const vpImage &I_RGBA_8U, const vpImage &buffer) +static bool readBinaryFile(const std::string &filename, std::vector &buffer) { std::FILE *f = std::fopen(filename.c_str(), "rb"); CHECK(f != nullptr); @@ -951,7 +951,7 @@ static bool reaBinaryFile(const std::string &filename, std::vector &bu return true; } -static bool reaBinaryFile(const std::string &filename, std::vector &buffer) +static bool readBinaryFile(const std::string &filename, std::vector &buffer) { std::FILE *f = std::fopen(filename.c_str(), "rb"); CHECK(f != nullptr); @@ -986,7 +986,7 @@ TEST_CASE("Bayer conversion", "[image_conversion]") { const std::string filename = vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "Bayer/Klimt_Bayer_560x558_BGGR_12bits.raw"); - if (reaBinaryFile(filename, buffer)) { + if (readBinaryFile(filename, buffer)) { col2im(buffer, I_Bayer_16U); SECTION("Bilinear") @@ -1017,7 +1017,7 @@ TEST_CASE("Bayer conversion", "[image_conversion]") { const std::string filename = vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "Bayer/Klimt_Bayer_560x558_GBRG_12bits.raw"); - if (reaBinaryFile(filename, buffer)) { + if (readBinaryFile(filename, buffer)) { col2im(buffer, I_Bayer_16U); SECTION("Bilinear") @@ -1048,7 +1048,7 @@ TEST_CASE("Bayer conversion", "[image_conversion]") { const std::string filename = vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "Bayer/Klimt_Bayer_560x558_GRBG_12bits.raw"); - if (reaBinaryFile(filename, buffer)) { + if (readBinaryFile(filename, buffer)) { col2im(buffer, I_Bayer_16U); SECTION("Bilinear") @@ -1079,7 +1079,7 @@ TEST_CASE("Bayer conversion", "[image_conversion]") { const std::string filename = vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "Bayer/Klimt_Bayer_560x558_RGGB_12bits.raw"); - if (reaBinaryFile(filename, buffer)) { + if (readBinaryFile(filename, buffer)) { col2im(buffer, I_Bayer_16U); SECTION("Bilinear") @@ -1118,7 +1118,7 @@ TEST_CASE("Bayer conversion", "[image_conversion]") const std::string filename = vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "Bayer/Klimt_Bayer_560x558_BGGR_08bits.raw"); - if (reaBinaryFile(filename, buffer)) { + if (readBinaryFile(filename, buffer)) { col2im(buffer, I_Bayer_8U); SECTION("Bilinear") @@ -1148,7 +1148,7 @@ TEST_CASE("Bayer conversion", "[image_conversion]") const std::string filename = vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "Bayer/Klimt_Bayer_560x558_GBRG_08bits.raw"); - if (reaBinaryFile(filename, buffer)) { + if (readBinaryFile(filename, buffer)) { col2im(buffer, I_Bayer_8U); SECTION("Bilinear") @@ -1178,7 +1178,7 @@ TEST_CASE("Bayer conversion", "[image_conversion]") const std::string filename = vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "Bayer/Klimt_Bayer_560x558_GRBG_08bits.raw"); - if (reaBinaryFile(filename, buffer)) { + if (readBinaryFile(filename, buffer)) { col2im(buffer, I_Bayer_8U); SECTION("Bilinear") @@ -1208,7 +1208,7 @@ TEST_CASE("Bayer conversion", "[image_conversion]") const std::string filename = vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "Bayer/Klimt_Bayer_560x558_RGGB_08bits.raw"); - if (reaBinaryFile(filename, buffer)) { + if (readBinaryFile(filename, buffer)) { col2im(buffer, I_Bayer_8U); SECTION("Bilinear") diff --git a/modules/vision/src/pose-estimation/vpPoseRansac.cpp b/modules/vision/src/pose-estimation/vpPoseRansac.cpp index b6b66b9537..371790988c 100644 --- a/modules/vision/src/pose-estimation/vpPoseRansac.cpp +++ b/modules/vision/src/pose-estimation/vpPoseRansac.cpp @@ -458,7 +458,7 @@ bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneo if (foundSolution) { unsigned int nbMinRandom = 4; - // std::cout << "Nombre d'inliers " << nbInliers << std::endl + // std::cout << "Nb inliers " << nbInliers << std::endl; // Display the random picked points /* From 7254ec5ca807d0387fa0398e2c6607c3afb231fc Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Tue, 9 Apr 2024 18:59:40 +0200 Subject: [PATCH 14/17] Satisfy quality rules --- modules/core/src/image/vpImageConvert.cpp | 74 ++--- modules/core/src/image/vpImageConvert_yuv.cpp | 272 ++++++++++++------ modules/core/src/tools/file/vpIoTools.cpp | 172 ++++++----- .../homography-estimation/vpHomography.cpp | 8 +- .../vpHomographyRansac.cpp | 103 +++++-- .../homography-estimation/vpHomographyVVS.cpp | 110 ++++--- 6 files changed, 460 insertions(+), 279 deletions(-) diff --git a/modules/core/src/image/vpImageConvert.cpp b/modules/core/src/image/vpImageConvert.cpp index 83db7d22ff..6913994fdb 100644 --- a/modules/core/src/image/vpImageConvert.cpp +++ b/modules/core/src/image/vpImageConvert.cpp @@ -1595,10 +1595,10 @@ void vpImageConvert::computeYCbCrLUT() while (index--) { int aux = index - 128; - vpImageConvert::vpCrr[index] = static_cast(364.6610 * aux) >> 8; - vpImageConvert::vpCgb[index] = static_cast(-89.8779 * aux) >> 8; - vpImageConvert::vpCgr[index] = static_cast(-185.8154 * aux) >> 8; - vpImageConvert::vpCbb[index] = static_cast(460.5724 * aux) >> 8; + vpImageConvert::vpCrr[index] = static_cast((364.6610 * aux) / 256); + vpImageConvert::vpCgb[index] = static_cast((-89.8779 * aux) / 256); + vpImageConvert::vpCgr[index] = static_cast((-185.8154 * aux) / 256); + vpImageConvert::vpCbb[index] = static_cast((460.5724 * aux) / 256); } YCbCrLUTcomputed = true; @@ -1641,7 +1641,7 @@ void vpImageConvert::YCbCrToRGB(unsigned char *ycbcr, unsigned char *rgb, unsign while (size--) { int val_r, val_g, val_b; - if (!(col++ % 2)) { + if (!(col % 2)) { cbv = pt_ycbcr + 1; crv = pt_ycbcr + 3; } @@ -1652,11 +1652,12 @@ void vpImageConvert::YCbCrToRGB(unsigned char *ycbcr, unsigned char *rgb, unsign vpDEBUG_TRACE(5, "[%d] R: %d G: %d B: %d\n", size, val_r, val_g, val_b); - *pt_rgb++ = (val_r < 0) ? 0u : ((val_r > 255) ? 255u : (unsigned char)val_r); // Red component. - *pt_rgb++ = (val_g < 0) ? 0u : ((val_g > 255) ? 255u : (unsigned char)val_g); // Green component. - *pt_rgb++ = (val_b < 0) ? 0u : ((val_b > 255) ? 255u : (unsigned char)val_b); // Blue component. + *pt_rgb++ = (val_r < 0) ? 0u : ((val_r > 255) ? 255u : static_cast(val_r)); // Red component. + *pt_rgb++ = (val_g < 0) ? 0u : ((val_g > 255) ? 255u : static_cast(val_g)); // Green component. + *pt_rgb++ = (val_b < 0) ? 0u : ((val_b > 255) ? 255u : static_cast(val_b)); // Blue component. pt_ycbcr += 2; + ++col; } } @@ -1700,7 +1701,7 @@ void vpImageConvert::YCbCrToRGBa(unsigned char *ycbcr, unsigned char *rgba, unsi while (size--) { int val_r, val_g, val_b; - if (!(col++ % 2)) { + if (!(col % 2)) { cbv = pt_ycbcr + 1; crv = pt_ycbcr + 3; } @@ -1711,12 +1712,13 @@ void vpImageConvert::YCbCrToRGBa(unsigned char *ycbcr, unsigned char *rgba, unsi vpDEBUG_TRACE(5, "[%d] R: %d G: %d B: %d\n", size, val_r, val_g, val_b); - *pt_rgba++ = (val_r < 0) ? 0u : ((val_r > 255) ? 255u : (unsigned char)val_r); // Red component. - *pt_rgba++ = (val_g < 0) ? 0u : ((val_g > 255) ? 255u : (unsigned char)val_g); // Green component. - *pt_rgba++ = (val_b < 0) ? 0u : ((val_b > 255) ? 255u : (unsigned char)val_b); // Blue component. + *pt_rgba++ = (val_r < 0) ? 0u : ((val_r > 255) ? 255u : static_cast(val_r)); // Red component. + *pt_rgba++ = (val_g < 0) ? 0u : ((val_g > 255) ? 255u : static_cast(val_g)); // Green component. + *pt_rgba++ = (val_b < 0) ? 0u : ((val_b > 255) ? 255u : static_cast(val_b)); // Blue component. *pt_rgba++ = vpRGBa::alpha_default; pt_ycbcr += 2; + ++col; } } @@ -1786,7 +1788,7 @@ void vpImageConvert::YCrCbToRGB(unsigned char *ycrcb, unsigned char *rgb, unsign while (size--) { int val_r, val_g, val_b; - if (!(col++ % 2)) { + if (!(col % 2)) { crv = pt_ycbcr + 1; cbv = pt_ycbcr + 3; } @@ -1797,11 +1799,12 @@ void vpImageConvert::YCrCbToRGB(unsigned char *ycrcb, unsigned char *rgb, unsign vpDEBUG_TRACE(5, "[%d] R: %d G: %d B: %d\n", size, val_r, val_g, val_b); - *pt_rgb++ = (val_r < 0) ? 0u : ((val_r > 255) ? 255u : (unsigned char)val_r); // Red component. - *pt_rgb++ = (val_g < 0) ? 0u : ((val_g > 255) ? 255u : (unsigned char)val_g); // Green component. - *pt_rgb++ = (val_b < 0) ? 0u : ((val_b > 255) ? 255u : (unsigned char)val_b); // Blue component. + *pt_rgb++ = (val_r < 0) ? 0u : ((val_r > 255) ? 255u : static_cast(val_r)); // Red component. + *pt_rgb++ = (val_g < 0) ? 0u : ((val_g > 255) ? 255u : static_cast(val_g)); // Green component. + *pt_rgb++ = (val_b < 0) ? 0u : ((val_b > 255) ? 255u : static_cast(val_b)); // Blue component. pt_ycbcr += 2; + ++col; } } @@ -1844,7 +1847,7 @@ void vpImageConvert::YCrCbToRGBa(unsigned char *ycrcb, unsigned char *rgba, unsi while (size--) { int val_r, val_g, val_b; - if (!(col++ % 2)) { + if (!(col % 2)) { crv = pt_ycbcr + 1; cbv = pt_ycbcr + 3; } @@ -1855,12 +1858,13 @@ void vpImageConvert::YCrCbToRGBa(unsigned char *ycrcb, unsigned char *rgba, unsi vpDEBUG_TRACE(5, "[%d] R: %d G: %d B: %d\n", size, val_r, val_g, val_b); - *pt_rgba++ = (val_r < 0) ? 0u : ((val_r > 255) ? 255u : (unsigned char)val_r); // Red component. - *pt_rgba++ = (val_g < 0) ? 0u : ((val_g > 255) ? 255u : (unsigned char)val_g); // Green component. - *pt_rgba++ = (val_b < 0) ? 0u : ((val_b > 255) ? 255u : (unsigned char)val_b); // Blue component. + *pt_rgba++ = (val_r < 0) ? 0u : ((val_r > 255) ? 255u : static_cast(val_r)); // Red component. + *pt_rgba++ = (val_g < 0) ? 0u : ((val_g > 255) ? 255u : static_cast(val_g)); // Green component. + *pt_rgba++ = (val_b < 0) ? 0u : ((val_b > 255) ? 255u : static_cast(val_b)); // Blue component. *pt_rgba++ = vpRGBa::alpha_default; pt_ycbcr += 2; + ++col; } } @@ -2018,23 +2022,23 @@ void vpImageConvert::merge(const vpImage *R, const vpImage mapOfWidths, mapOfHeights; if (R != nullptr) { - mapOfWidths[R->getWidth()]++; - mapOfHeights[R->getHeight()]++; + ++mapOfWidths[R->getWidth()]; + ++mapOfHeights[R->getHeight()]; } if (G != nullptr) { - mapOfWidths[G->getWidth()]++; - mapOfHeights[G->getHeight()]++; + ++mapOfWidths[G->getWidth()]; + ++mapOfHeights[G->getHeight()]; } if (B != nullptr) { - mapOfWidths[B->getWidth()]++; - mapOfHeights[B->getHeight()]++; + ++mapOfWidths[B->getWidth()]; + ++mapOfHeights[B->getHeight()]; } if (a != nullptr) { - mapOfWidths[a->getWidth()]++; - mapOfHeights[a->getHeight()]++; + ++mapOfWidths[a->getWidth()]; + ++mapOfHeights[a->getHeight()]; } if ((mapOfWidths.size() == 1) && (mapOfHeights.size() == 1)) { @@ -2090,12 +2094,13 @@ void vpImageConvert::merge(const vpImage *R, const vpImage(size) * 2) - 1; + int j = static_cast(size) - 1; while (i >= 0) { int y = grey16[i--]; - grey[j--] = static_cast((y + (grey16[i--] << 8)) >> 8); + grey[j--] = static_cast((y + (grey16[i] * 256)) / 256); + --i; } } @@ -2112,12 +2117,13 @@ void vpImageConvert::MONO16ToGrey(unsigned char *grey16, unsigned char *grey, un */ void vpImageConvert::MONO16ToRGBa(unsigned char *grey16, unsigned char *rgba, unsigned int size) { - int i = (((int)size) << 1) - 1; - int j = static_cast(size * 4 - 1); + int i = (static_cast(size) * 2) - 1; + int j = (static_cast(size) * 4) - 1; while (i >= 0) { int y = grey16[i--]; - unsigned char v = static_cast((y + (grey16[i--] << 8)) >> 8); + unsigned char v = static_cast((y + (grey16[i] * 256)) / 256); + --i; rgba[j--] = vpRGBa::alpha_default; rgba[j--] = v; rgba[j--] = v; diff --git a/modules/core/src/image/vpImageConvert_yuv.cpp b/modules/core/src/image/vpImageConvert_yuv.cpp index b2ac34c5ac..bec8f5a03a 100644 --- a/modules/core/src/image/vpImageConvert_yuv.cpp +++ b/modules/core/src/image/vpImageConvert_yuv.cpp @@ -41,13 +41,19 @@ #include #include -#define vpSAT(c) \ - if (c & (~255)) { \ - if (c < 0) \ - {c = 0;} \ - else \ - {c = 255;} \ +namespace +{ +void vpSAT(int &c) +{ + if (c < 0) { + c = 0; } + else { + c = 255; + } +} +}; + /*! Convert an image from YUYV 4:2:2 (y0 u01 y1 v01 y2 u23 y3 v23 ...) to RGB32. Destination rgba memory area has to be allocated before. @@ -74,12 +80,16 @@ void vpImageConvert::YUYVToRGBa(unsigned char *yuyv, unsigned char *rgba, unsign while (h--) { int c = w / 2; while (c--) { - y1 = *s++; + y1 = *s; + ++s; cb = ((*s - 128) * 454) / 256; - cg = (*s++ - 128) * 88; - y2 = *s++; + cg = (*s - 128) * 88; + ++s; + y2 = *s; + ++s; cr = ((*s - 128) * 359) / 256; - cg = (cg + ((*s++ - 128) * 183)) / 256; + cg = (cg + ((*s - 128) * 183)) / 256; + ++s; r = y1 + cr; b = y1 + cb; @@ -132,17 +142,23 @@ void vpImageConvert::YUYVToRGB(unsigned char *yuyv, unsigned char *rgb, unsigned while (h--) { int c = w / 2; while (c--) { - y1 = *s++; + y1 = *s; + ++s; cb = ((*s - 128) * 454) / 256; - cg = (*s++ - 128) * 88; - y2 = *s++; + cg = (*s - 128) * 88; + ++s; + y2 = *s; + ++s; cr = ((*s - 128) * 359) / 256; - cg = (cg + ((*s++ - 128) * 183)) / 256; + cg = (cg + ((*s - 128) * 183)) / 256; + ++s; r = y1 + cr; b = y1 + cb; g = y1 - cg; - vpSAT(r); vpSAT(g); vpSAT(b); + vpSAT(r); + vpSAT(g); + vpSAT(b); *d++ = static_cast(r); *d++ = static_cast(g); @@ -151,7 +167,9 @@ void vpImageConvert::YUYVToRGB(unsigned char *yuyv, unsigned char *rgb, unsigned r = y2 + cr; b = y2 + cb; g = y2 - cg; - vpSAT(r); vpSAT(g); vpSAT(b); + vpSAT(r); + vpSAT(g); + vpSAT(b); *d++ = static_cast(r); *d++ = static_cast(g); @@ -193,14 +211,20 @@ void vpImageConvert::YUYVToGrey(unsigned char *yuyv, unsigned char *grey, unsign void vpImageConvert::YUV411ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int size) { for (unsigned int i = size / 4; i; --i) { - int U = static_cast((*yuv++ - 128) * 0.354); + int U = static_cast((*yuv - 128) * 0.354); + ++yuv; int U5 = 5 * U; - int Y0 = *yuv++; - int Y1 = *yuv++; - int V = static_cast((*yuv++ - 128) * 0.707); + int Y0 = *yuv; + ++yuv; + int Y1 = *yuv; + ++yuv; + int V = static_cast((*yuv - 128) * 0.707); + ++yuv; int V2 = 2 * V; - int Y2 = *yuv++; - int Y3 = *yuv++; + int Y2 = *yuv; + ++yuv; + int Y3 = *yuv; + ++yuv; int UV = -U - V; // Original equations @@ -283,12 +307,16 @@ void vpImageConvert::YUV411ToRGBa(unsigned char *yuv, unsigned char *rgba, unsig void vpImageConvert::YUV422ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int size) { for (unsigned int i = size / 2; i; --i) { - int U = static_cast((*yuv++ - 128) * 0.354); + int U = static_cast((*yuv - 128) * 0.354); + ++yuv; int U5 = 5 * U; - int Y0 = *yuv++; - int V = static_cast((*yuv++ - 128) * 0.707); + int Y0 = *yuv; + ++yuv; + int V = static_cast((*yuv - 128) * 0.707); + ++yuv; int V2 = 2 * V; - int Y1 = *yuv++; + int Y1 = *yuv; + ++yuv; int UV = -U - V; //--- @@ -360,12 +388,16 @@ void vpImageConvert::YUV411ToGrey(unsigned char *yuv, unsigned char *grey, unsig void vpImageConvert::YUV422ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int size) { for (unsigned int i = size / 2; i; --i) { - int U = static_cast((*yuv++ - 128) * 0.354); + int U = static_cast((*yuv - 128) * 0.354); + ++yuv; int U5 = 5 * U; - int Y0 = *yuv++; - int V = static_cast((*yuv++ - 128) * 0.707); + int Y0 = *yuv; + ++yuv; + int V = static_cast((*yuv - 128) * 0.707); + ++yuv; int V2 = 2 * V; - int Y1 = *yuv++; + int Y1 = *yuv; + ++yuv; int UV = -U - V; //--- @@ -431,14 +463,20 @@ void vpImageConvert::YUV422ToGrey(unsigned char *yuv, unsigned char *grey, unsig void vpImageConvert::YUV411ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int size) { for (unsigned int i = size / 4; i; --i) { - int U = static_cast((*yuv++ - 128) * 0.354); + int U = static_cast((*yuv - 128) * 0.354); + ++yuv; int U5 = 5 * U; - int Y0 = *yuv++; - int Y1 = *yuv++; - int V = static_cast((*yuv++ - 128) * 0.707); + int Y0 = *yuv; + ++yuv; + int Y1 = *yuv; + ++yuv; + int V = static_cast((*yuv - 128) * 0.707); + ++yuv; int V2 = 2 * V; - int Y2 = *yuv++; - int Y3 = *yuv++; + int Y2 = *yuv; + ++yuv; + int Y3 = *yuv; + ++yuv; int UV = -U - V; // Original equations @@ -514,7 +552,6 @@ void vpImageConvert::YUV411ToRGB(unsigned char *yuv, unsigned char *rgb, unsigne */ void vpImageConvert::YUV420ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int width, unsigned int height) { - // std::cout << "call optimized ConvertYUV420ToRGBa()" << std::endl; int U, V, R, G, B, V2, U5, UV; int Y0, Y1, Y2, Y3; unsigned int size = width * height; @@ -523,15 +560,19 @@ void vpImageConvert::YUV420ToRGBa(unsigned char *yuv, unsigned char *rgba, unsig const unsigned int halfHeight = height / 2, halfWidth = width / 2; for (unsigned int i = 0; i < halfHeight; ++i) { for (unsigned int j = 0; j < halfWidth; ++j) { - U = static_cast((*iU++ - 128) * 0.354); + U = static_cast(((*iU) - 128) * 0.354); + ++iU; U5 = 5 * U; - V = static_cast((*iV++ - 128) * 0.707); + V = static_cast(((*iV) - 128) * 0.707); + ++iV; V2 = 2 * V; UV = -U - V; - Y0 = *yuv++; + Y0 = *yuv; + ++yuv; Y1 = *yuv; yuv = yuv + (width - 1); - Y2 = *yuv++; + Y2 = *yuv; + ++yuv; Y3 = *yuv; yuv = (yuv - width) + 1; @@ -567,7 +608,7 @@ void vpImageConvert::YUV420ToRGBa(unsigned char *yuv, unsigned char *rgba, unsig *rgba++ = static_cast(G); *rgba++ = static_cast(B); *rgba = vpRGBa::alpha_default; - rgba = rgba + 4 * width - 7; + rgba = (rgba + (4 * width)) - 7; //--- R = Y2 + V2; @@ -623,15 +664,19 @@ void vpImageConvert::YUV420ToRGB(unsigned char *yuv, unsigned char *rgb, unsigne const unsigned int halfHeight = height / 2, halfWidth = width / 2; for (unsigned int i = 0; i < halfHeight; ++i) { for (unsigned int j = 0; j < halfWidth; ++j) { - U = static_cast((*iU++ - 128) * 0.354); + U = static_cast(((*iU) - 128) * 0.354); + ++iU; U5 = 5 * U; - V = static_cast((*iV++ - 128) * 0.707); + V = static_cast(((*iV) - 128) * 0.707); + ++iV; V2 = 2 * V; UV = -U - V; - Y0 = *yuv++; + Y0 = *yuv; + ++yuv; Y1 = *yuv; yuv = yuv + (width - 1); - Y2 = *yuv++; + Y2 = *yuv; + ++yuv; Y3 = *yuv; yuv = (yuv - width) + 1; @@ -711,7 +756,8 @@ void vpImageConvert::YUV420ToRGB(unsigned char *yuv, unsigned char *rgb, unsigne void vpImageConvert::YUV420ToGrey(unsigned char *yuv, unsigned char *grey, unsigned int size) { for (unsigned int i = 0; i < size; ++i) { - *grey++ = *yuv++; + *grey++ = *yuv; + ++yuv; } } @@ -727,10 +773,13 @@ void vpImageConvert::YUV420ToGrey(unsigned char *yuv, unsigned char *grey, unsig void vpImageConvert::YUV444ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int size) { for (unsigned int i = 0; i < size; ++i) { - int U = static_cast((*yuv++ - 128) * 0.354); + int U = static_cast((*yuv - 128) * 0.354); + ++yuv; int U5 = 5 * U; - int Y = *yuv++; - int V = static_cast((*yuv++ - 128) * 0.707); + int Y = *yuv; + ++yuv; + int V = static_cast((*yuv - 128) * 0.707); + ++yuv; int V2 = 2 * V; int UV = -U - V; @@ -764,10 +813,13 @@ void vpImageConvert::YUV444ToRGBa(unsigned char *yuv, unsigned char *rgba, unsig void vpImageConvert::YUV444ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int size) { for (unsigned int i = 0; i < size; ++i) { - int U = static_cast((*yuv++ - 128) * 0.354); + int U = static_cast((*yuv - 128) * 0.354); + ++yuv; int U5 = 5 * U; - int Y = *yuv++; - int V = static_cast((*yuv++ - 128) * 0.707); + int Y = *yuv; + ++yuv; + int V = static_cast((*yuv - 128) * 0.707); + ++yuv; int V2 = 2 * V; int UV = -U - V; @@ -818,7 +870,6 @@ void vpImageConvert::YUV444ToGrey(unsigned char *yuv, unsigned char *grey, unsig */ void vpImageConvert::YV12ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int width, unsigned int height) { - // std::cout << "call optimized ConvertYV12ToRGBa()" << std::endl; int U, V, R, G, B, V2, U5, UV; int Y0, Y1, Y2, Y3; unsigned int size = width * height; @@ -827,15 +878,19 @@ void vpImageConvert::YV12ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne const unsigned int halfHeight = height / 2, halfWidth = width / 2; for (unsigned int i = 0; i < halfHeight; ++i) { for (unsigned int j = 0; j < halfWidth; ++j) { - U = static_cast((*iU++ - 128) * 0.354); + U = static_cast(((*iU) - 128) * 0.354); + ++iU; U5 = 5 * U; - V = static_cast((*iV++ - 128) * 0.707); + V = static_cast(((*iV) - 128) * 0.707); + ++iV; V2 = 2 * V; UV = -U - V; - Y0 = *yuv++; + Y0 = *yuv; + ++yuv; Y1 = *yuv; yuv = yuv + (width - 1); - Y2 = *yuv++; + Y2 = *yuv; + ++yuv; Y3 = *yuv; yuv = (yuv - width) + 1; @@ -919,7 +974,6 @@ void vpImageConvert::YV12ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne */ void vpImageConvert::YV12ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int height, unsigned int width) { - // std::cout << "call optimized ConvertYV12ToRGB()" << std::endl; int U, V, R, G, B, V2, U5, UV; int Y0, Y1, Y2, Y3; unsigned int size = width * height; @@ -928,15 +982,19 @@ void vpImageConvert::YV12ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned const unsigned int halfHeight = height / 2, halfWidth = width / 2; for (unsigned int i = 0; i < halfHeight; ++i) { for (unsigned int j = 0; j < halfWidth; ++j) { - U = static_cast((*iU++ - 128) * 0.354); + U = static_cast(((*iU) - 128) * 0.354); + ++iU; U5 = 5 * U; - V = static_cast((*iV++ - 128) * 0.707); + V = static_cast(((*iV) - 128) * 0.707); + ++iV; V2 = 2 * V; UV = -U - V; - Y0 = *yuv++; + Y0 = *yuv; + ++yuv; Y1 = *yuv; yuv = yuv + (width - 1); - Y2 = *yuv++; + Y2 = *yuv; + ++yuv; Y3 = *yuv; yuv = (yuv - width) + 1; @@ -1018,7 +1076,6 @@ void vpImageConvert::YV12ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned */ void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int width, unsigned int height) { - // std::cout << "call optimized ConvertYVU9ToRGBa()" << std::endl; int U, V, R, G, B, V2, U5, UV; int Y0, Y1, Y2, Y3, Y4, Y5, Y6, Y7, Y8, Y9, Y10, Y11, Y12, Y13, Y14, Y15; unsigned int size = width * height; @@ -1027,29 +1084,43 @@ void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne const unsigned int quarterHeight = height / 4, quarterWidth = width / 4; for (unsigned int i = 0; i < quarterHeight; ++i) { for (unsigned int j = 0; j < quarterWidth; ++j) { - U = static_cast((*iU++ - 128) * 0.354); + U = static_cast(((*iU) - 128) * 0.354); + ++iU; U5 = 5 * U; - V = static_cast((*iV++ - 128) * 0.707); + V = static_cast(((*iV) - 128) * 0.707); + ++iV; V2 = 2 * V; UV = -U - V; - Y0 = *yuv++; - Y1 = *yuv++; - Y2 = *yuv++; + Y0 = *yuv; + ++yuv; + Y1 = *yuv; + ++yuv; + Y2 = *yuv; + ++yuv; Y3 = *yuv; yuv = yuv + (width - 3); - Y4 = *yuv++; - Y5 = *yuv++; - Y6 = *yuv++; + Y4 = *yuv; + ++yuv; + Y5 = *yuv; + ++yuv; + Y6 = *yuv; + ++yuv; Y7 = *yuv; yuv = yuv + (width - 3); - Y8 = *yuv++; - Y9 = *yuv++; - Y10 = *yuv++; + Y8 = *yuv; + ++yuv; + Y9 = *yuv; + ++yuv; + Y10 = *yuv; + ++yuv; Y11 = *yuv; yuv = yuv + (width - 3); - Y12 = *yuv++; - Y13 = *yuv++; - Y14 = *yuv++; + Y12 = *yuv; + ++yuv; + Y13 = *yuv; + ++yuv; + Y14 = *yuv; + ++yuv; Y15 = *yuv; yuv = (yuv - (3 * width)) + 1; @@ -1311,7 +1382,6 @@ void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne */ void vpImageConvert::YVU9ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int height, unsigned int width) { - // std::cout << "call optimized ConvertYVU9ToRGB()" << std::endl; int U, V, R, G, B, V2, U5, UV; int Y0, Y1, Y2, Y3, Y4, Y5, Y6, Y7, Y8, Y9, Y10, Y11, Y12, Y13, Y14, Y15; unsigned int size = width * height; @@ -1320,29 +1390,43 @@ void vpImageConvert::YVU9ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned const unsigned int quarterHeight = height / 4, quarterWidth = width / 4; for (unsigned int i = 0; i < quarterHeight; ++i) { for (unsigned int j = 0; j < quarterWidth; ++j) { - U = static_cast((*iU++ - 128) * 0.354); + U = static_cast((*iU - 128) * 0.354); + ++iU; U5 = 5 * U; - V = static_cast((*iV++ - 128) * 0.707); + V = static_cast((*iV - 128) * 0.707); + ++iV; V2 = 2 * V; UV = -U - V; - Y0 = *yuv++; - Y1 = *yuv++; - Y2 = *yuv++; + Y0 = *yuv; + ++yuv; + Y1 = *yuv; + ++yuv; + Y2 = *yuv; + ++yuv; Y3 = *yuv; yuv = yuv + (width - 3); - Y4 = *yuv++; - Y5 = *yuv++; - Y6 = *yuv++; + Y4 = *yuv; + ++yuv; + Y5 = *yuv; + ++yuv; + Y6 = *yuv; + ++yuv; Y7 = *yuv; yuv = yuv + (width - 3); - Y8 = *yuv++; - Y9 = *yuv++; - Y10 = *yuv++; + Y8 = *yuv; + ++yuv; + Y9 = *yuv; + ++yuv; + Y10 = *yuv; + ++yuv; Y11 = *yuv; yuv = yuv + (width - 3); - Y12 = *yuv++; - Y13 = *yuv++; - Y14 = *yuv++; + Y12 = *yuv; + ++yuv; + Y13 = *yuv; + ++yuv; + Y14 = *yuv; + ++yuv; Y15 = *yuv; yuv = (yuv - (3 * width)) + 1; @@ -1516,7 +1600,7 @@ void vpImageConvert::YVU9ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned *rgb++ = static_cast(R); *rgb++ = static_cast(G); *rgb = static_cast(B); - rgb = rgb + 3 * width - 11; + rgb = (rgb + (3 * width)) - 11; R = Y12 + V2; vpSAT(R); diff --git a/modules/core/src/tools/file/vpIoTools.cpp b/modules/core/src/tools/file/vpIoTools.cpp index f4b25f22af..e00bd664d6 100644 --- a/modules/core/src/tools/file/vpIoTools.cpp +++ b/modules/core/src/tools/file/vpIoTools.cpp @@ -30,8 +30,7 @@ * * Description: * Directory management. - * -*****************************************************************************/ + */ /*! \file vpIoTools.cpp @@ -119,7 +118,7 @@ using namespace buminiz; #endif // To avoid warnings such as: warning: unused variable ‘littleEndian’ [-Wunused-variable] -#define _unused(x) ((void)(x)) // see: https://stackoverflow.com/a/777359 +#define UNUSED(x) ((void)(x)) // see: https://stackoverflow.com/a/777359 // Copyright (C) 2011 Carl Rogers // Released under MIT License @@ -130,41 +129,38 @@ using namespace buminiz; char visp::cnpy::BigEndianTest() { int x = 1; - return (((char *)&x)[0]) ? '<' : '>'; + return (((reinterpret_cast(&x))[0]) ? '<' : '>'); } char visp::cnpy::map_type(const std::type_info &t) { - if (t == typeid(float)) return 'f'; - if (t == typeid(double)) return 'f'; - if (t == typeid(long double)) return 'f'; + if (t == typeid(float)) { return 'f'; } + if (t == typeid(double)) { return 'f'; } + if (t == typeid(long double)) { return 'f'; } - if (t == typeid(int)) return 'i'; - if (t == typeid(char)) return 'i'; - if (t == typeid(short)) return 'i'; - if (t == typeid(long)) return 'i'; - if (t == typeid(long long)) return 'i'; + if (t == typeid(int)) { return 'i'; } + if (t == typeid(char)) { return 'i'; } + if (t == typeid(short)) { return 'i'; } + if (t == typeid(long)) { return 'i'; } + if (t == typeid(long long)) { return 'i'; } - if (t == typeid(unsigned char)) return 'u'; - if (t == typeid(unsigned short)) return 'u'; - if (t == typeid(unsigned long)) return 'u'; - if (t == typeid(unsigned long long)) return 'u'; - if (t == typeid(unsigned int)) return 'u'; + if (t == typeid(unsigned char)) { return 'u'; } + if (t == typeid(unsigned short)) { return 'u'; } + if (t == typeid(unsigned long)) { return 'u'; } + if (t == typeid(unsigned long long)) { return 'u'; } + if (t == typeid(unsigned int)) { return 'u'; } - if (t == typeid(bool)) return 'b'; + if (t == typeid(bool)) { return 'b'; } - if (t == typeid(std::complex)) return 'c'; - if (t == typeid(std::complex)) return 'c'; - if (t == typeid(std::complex)) return 'c'; + if (t == typeid(std::complex)) { return 'c'; } + if (t == typeid(std::complex)) { return 'c'; } + if (t == typeid(std::complex)) { return 'c'; } - else return '?'; + else { return '?'; } } void visp::cnpy::parse_npy_header(unsigned char *buffer, size_t &word_size, std::vector &shape, bool &fortran_order) { - //std::string magic_string(buffer,6); - // uint8_t major_version = *reinterpret_cast(buffer+6); - // uint8_t minor_version = *reinterpret_cast(buffer+7); uint16_t header_len = *reinterpret_cast(buffer+8); std::string header(reinterpret_cast(buffer+9), header_len); @@ -190,11 +186,8 @@ void visp::cnpy::parse_npy_header(unsigned char *buffer, size_t &word_size, std: //byte order code | stands for not applicable. //not sure when this applies except for byte array loc1 = header.find("descr")+9; - bool littleEndian = (header[loc1] == '<' || header[loc1] == '|' ? true : false); - _unused(littleEndian); assert(littleEndian); - - //char type = header[loc1+1]; - //assert(type == map_type(T)); + bool littleEndian = (((header[loc1] == '<') || (header[loc1] == '|')) ? true : false); + UNUSED(littleEndian); assert(littleEndian); std::string str_ws = header.substr(loc1+2); loc2 = str_ws.find("'"); @@ -205,8 +198,9 @@ void visp::cnpy::parse_npy_header(FILE *fp, size_t &word_size, std::vector footer(22); fseek(fp, -22, SEEK_END); size_t res = fread(&footer[0], sizeof(char), 22, fp); - if (res != 22) + if (res != 22) { throw std::runtime_error("parse_zip_footer: failed fread"); + } uint16_t disk_no, disk_start, nrecs_on_disk, comment_len; disk_no = *(uint16_t *)&footer[4]; @@ -270,10 +268,10 @@ void visp::cnpy::parse_zip_footer(FILE *fp, uint16_t &nrecs, size_t &global_head global_header_offset = *(uint32_t *)&footer[16]; comment_len = *(uint16_t *)&footer[20]; - _unused(disk_no); assert(disk_no == 0); - _unused(disk_start); assert(disk_start == 0); - _unused(nrecs_on_disk); assert(nrecs_on_disk == nrecs); - _unused(comment_len); assert(comment_len == 0); + UNUSED(disk_no); assert(disk_no == 0); + UNUSED(disk_start); assert(disk_start == 0); + UNUSED(nrecs_on_disk); assert(nrecs_on_disk == nrecs); + UNUSED(comment_len); assert(comment_len == 0); } visp::cnpy::NpyArray load_the_npy_file(FILE *fp) @@ -285,8 +283,9 @@ visp::cnpy::NpyArray load_the_npy_file(FILE *fp) visp::cnpy::NpyArray arr(shape, word_size, fortran_order); size_t nread = fread(arr.data(), 1, arr.num_bytes(), fp); - if (nread != arr.num_bytes()) + if (nread != arr.num_bytes()) { throw std::runtime_error("load_the_npy_file: failed fread"); + } return arr; } @@ -295,8 +294,9 @@ visp::cnpy::NpyArray load_the_npz_array(FILE *fp, uint32_t compr_bytes, uint32_t std::vector buffer_compr(compr_bytes); std::vector buffer_uncompr(uncompr_bytes); size_t nread = fread(&buffer_compr[0], 1, compr_bytes, fp); - if (nread != compr_bytes) + if (nread != compr_bytes) { throw std::runtime_error("load_the_npy_file: failed fread"); + } z_stream d_stream; @@ -306,7 +306,7 @@ visp::cnpy::NpyArray load_the_npz_array(FILE *fp, uint32_t compr_bytes, uint32_t d_stream.avail_in = 0; d_stream.next_in = Z_NULL; int err = inflateInit2(&d_stream, -MAX_WBITS); - _unused(err); assert(err == 0); + UNUSED(err); assert(err == 0); d_stream.avail_in = compr_bytes; d_stream.next_in = &buffer_compr[0]; @@ -314,9 +314,9 @@ visp::cnpy::NpyArray load_the_npz_array(FILE *fp, uint32_t compr_bytes, uint32_t d_stream.next_out = &buffer_uncompr[0]; err = inflate(&d_stream, Z_FINISH); - _unused(err); assert(err == 0); + UNUSED(err); assert(err == 0); err = inflateEnd(&d_stream); - _unused(err); assert(err == 0); + UNUSED(err); assert(err == 0); std::vector shape; size_t word_size; @@ -348,22 +348,26 @@ visp::cnpy::npz_t visp::cnpy::npz_load(std::string fname) } visp::cnpy::npz_t arrays; - - while (1) { + bool quit = false; + while (!quit) { std::vector local_header(30); size_t headerres = fread(&local_header[0], sizeof(char), 30, fp); - if (headerres != 30) + if (headerres != 30) { throw std::runtime_error("npz_load: failed fread"); + } //if we've reached the global header, stop reading - if (local_header[2] != 0x03 || local_header[3] != 0x04) break; + if ((local_header[2] != 0x03) || (local_header[3] != 0x04)) { + quit = true; + }; //read in the variable name uint16_t name_len = *(uint16_t *)&local_header[26]; std::string varname(name_len, ' '); size_t vname_res = fread(&varname[0], sizeof(char), name_len, fp); - if (vname_res != name_len) + if (vname_res != name_len) { throw std::runtime_error("npz_load: failed fread"); + } //erase the lagging .npy varname.erase(varname.end()-4, varname.end()); @@ -373,8 +377,9 @@ visp::cnpy::npz_t visp::cnpy::npz_load(std::string fname) if (extra_field_len > 0) { std::vector buff(extra_field_len); size_t efield_res = fread(&buff[0], sizeof(char), extra_field_len, fp); - if (efield_res != extra_field_len) + if (efield_res != extra_field_len) { throw std::runtime_error("npz_load: failed fread"); + } } uint16_t compr_method = *reinterpret_cast(&local_header[0]+8); @@ -402,23 +407,30 @@ visp::cnpy::NpyArray visp::cnpy::npz_load(std::string fname, std::string varname { FILE *fp = fopen(fname.c_str(), "rb"); - if (!fp) throw std::runtime_error("npz_load: Unable to open file "+fname); + if (!fp) { + throw std::runtime_error("npz_load: Unable to open file "+fname); + } - while (1) { + bool quit = false; + while (!quit) { std::vector local_header(30); size_t header_res = fread(&local_header[0], sizeof(char), 30, fp); - if (header_res != 30) + if (header_res != 30) { throw std::runtime_error("npz_load: failed fread"); + } - //if we've reached the global header, stop reading - if (local_header[2] != 0x03 || local_header[3] != 0x04) break; + //if we've reached the global header, stop reading + if (local_header[2] != 0x03 || local_header[3] != 0x04) { + quit = true; + }; //read in the variable name uint16_t name_len = *(uint16_t *)&local_header[26]; std::string vname(name_len, ' '); size_t vname_res = fread(&vname[0], sizeof(char), name_len, fp); - if (vname_res != name_len) + if (vname_res != name_len) { throw std::runtime_error("npz_load: failed fread"); + } vname.erase(vname.end()-4, vname.end()); //erase the lagging .npy //read in the extra field @@ -460,7 +472,9 @@ visp::cnpy::NpyArray visp::cnpy::npy_load(std::string fname) FILE *fp = fopen(fname.c_str(), "rb"); - if (!fp) throw std::runtime_error("npy_load: Unable to open file "+fname); + if (!fp) { + throw std::runtime_error("npy_load: Unable to open file "+fname); + } NpyArray arr = load_the_npy_file(fp); @@ -665,12 +679,12 @@ void vpIoTools::getUserName(std::string &username) // With MinGW, UNIX and _WIN32 are defined #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX // Get the user name. - char *_username = ::getenv("LOGNAME"); - if (!_username) { + char *logname = ::getenv("LOGNAME"); + if (!logname) { username = "unknown"; } else { - username = _username; + username = logname; } #elif defined(_WIN32) #if (!defined(WINRT)) @@ -829,18 +843,18 @@ bool vpIoTools::checkDirectory(const std::string &dirname) return false; } - std::string _dirname = path(dirname); + std::string path_dirname = path(dirname); - if (VP_STAT(_dirname.c_str(), &stbuf) != 0) { + if (VP_STAT(path_dirname.c_str(), &stbuf) != 0) { // Test adding the separator if not already present - if (_dirname.at(_dirname.size() - 1) != separator) { - if (VP_STAT((_dirname + separator).c_str(), &stbuf) != 0) { + if (path_dirname.at(path_dirname.size() - 1) != separator) { + if (VP_STAT((path_dirname + separator).c_str(), &stbuf) != 0) { return false; } } // Test removing the separator if already present - if (_dirname.at(_dirname.size() - 1) == separator) { - if (VP_STAT((_dirname.substr(0, _dirname.size() - 1)).c_str(), &stbuf) != 0) { + if (path_dirname.at(path_dirname.size() - 1) == separator) { + if (VP_STAT((path_dirname.substr(0, path_dirname.size() - 1)).c_str(), &stbuf) != 0) { return false; } } @@ -909,36 +923,36 @@ int vpIoTools::mkdir_p(const std::string &path, int mode) } // Iterate over the string - std::string _path = path; - std::string _sub_path; - for (size_t pos = 0; (pos = _path.find(vpIoTools::separator)) != std::string::npos;) { - _sub_path += _path.substr(0, pos + 1); + std::string cpy_path = path; + std::string sub_path; + for (size_t pos = 0; (pos = cpy_path.find(vpIoTools::separator)) != std::string::npos;) { + sub_path += cpy_path.substr(0, pos + 1); // Continue if sub_path = separator if (pos == 0) { - _path.erase(0, pos + 1); + cpy_path.erase(0, pos + 1); continue; } #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) - if (mkdir(_sub_path.c_str(), static_cast(mode)) != 0) + if (mkdir(sub_path.c_str(), static_cast(mode)) != 0) #elif defined(_WIN32) (void)mode; // var not used - if (!checkDirectory(_sub_path) && _mkdir(_sub_path.c_str()) != 0) + if (!checkDirectory(sub_path) && _mkdir(sub_path.c_str()) != 0) #endif { if (errno != EEXIST) { return -1; } } - _path.erase(0, pos + 1); + cpy_path.erase(0, pos + 1); } - if (!_path.empty()) { - _sub_path += _path; + if (!cpy_path.empty()) { + sub_path += cpy_path; #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) - if (mkdir(_sub_path.c_str(), static_cast(mode)) != 0) + if (mkdir(sub_path.c_str(), static_cast(mode)) != 0) #elif defined(_WIN32) - if (_mkdir(_sub_path.c_str()) != 0) + if (_mkdir(sub_path.c_str()) != 0) #endif { if (errno != EEXIST) { diff --git a/modules/vision/src/homography-estimation/vpHomography.cpp b/modules/vision/src/homography-estimation/vpHomography.cpp index 493db75ef9..c2f421fa6c 100644 --- a/modules/vision/src/homography-estimation/vpHomography.cpp +++ b/modules/vision/src/homography-estimation/vpHomography.cpp @@ -257,7 +257,7 @@ vpHomography &vpHomography::operator/=(double v) vpHomography &vpHomography::operator=(const vpHomography &H) { const unsigned int nbCols = 3, nbRows = 3; - for (unsigned int i = 0; i < nbRows; ++i){ + for (unsigned int i = 0; i < nbRows; ++i) { for (unsigned int j = 0; j < nbCols; ++j) { (*this)[i][j] = H[i][j]; } @@ -370,9 +370,9 @@ void vpHomography::build(vpHomography &aHb, const vpHomogeneousMatrix &aMb, cons double vpHomography::det() const { - return ((*this)[0][0] * (((*this)[1][1] * (*this)[2][2]) - ((*this)[1][2] * (*this)[2][1])) - - (*this)[0][1] * (((*this)[1][0] * (*this)[2][2]) - ((*this)[1][2] * (*this)[2][0])) + - (*this)[0][2] * (((*this)[1][0] * (*this)[2][1]) - ((*this)[1][1] * (*this)[2][0]))); + return ((((*this)[0][0] * (((*this)[1][1] * (*this)[2][2]) - ((*this)[1][2] * (*this)[2][1]))) - + ((*this)[0][1] * (((*this)[1][0] * (*this)[2][2]) - ((*this)[1][2] * (*this)[2][0])))) + + ((*this)[0][2] * (((*this)[1][0] * (*this)[2][1]) - ((*this)[1][1] * (*this)[2][0])))); } void vpHomography::eye() diff --git a/modules/vision/src/homography-estimation/vpHomographyRansac.cpp b/modules/vision/src/homography-estimation/vpHomographyRansac.cpp index 816b71785d..bd0e5fae50 100644 --- a/modules/vision/src/homography-estimation/vpHomographyRansac.cpp +++ b/modules/vision/src/homography-estimation/vpHomographyRansac.cpp @@ -95,42 +95,69 @@ bool vpHomography::degenerateConfiguration(const vpColVector &x, unsigned int *i i = 0, j = 1, k = 2; - double area012 = ((-pa[j][0] * pa[i][1]) + (pa[k][0] * pa[i][1]) + (pa[i][0] * pa[j][1]) - (pa[k][0] * pa[j][1]) + - (-pa[i][0] * pa[k][1]) + (pa[1][j] * pa[k][1])); + double area012 = ((((((-pa[j][0] * pa[i][1]) + (pa[k][0] * pa[i][1])) + (pa[i][0] * pa[j][1])) - (pa[k][0] * pa[j][1])) + + (-pa[i][0] * pa[k][1])) + (pa[1][j] * pa[k][1])); i = 0; j = 1, k = 3; - double area013 = ((-pa[j][0] * pa[i][1]) + (pa[k][0] * pa[i][1]) + (pa[i][0] * pa[j][1]) - (pa[k][0] * pa[j][1]) + - (-pa[i][0] * pa[k][1]) + (pa[1][j] * pa[k][1])); + double area013 = ((((((-pa[j][0] * pa[i][1]) + (pa[k][0] * pa[i][1])) + (pa[i][0] * pa[j][1])) - (pa[k][0] * pa[j][1])) + + (-pa[i][0] * pa[k][1])) + (pa[1][j] * pa[k][1])); i = 0; j = 2, k = 3; - double area023 = ((-pa[j][0] * pa[i][1]) + (pa[k][0] * pa[i][1]) + (pa[i][0] * pa[j][1]) - (pa[k][0] * pa[j][1]) + - (-pa[i][0] * pa[k][1]) + (pa[1][j] * pa[k][1])); + double area023 = ((((((-pa[j][0] * pa[i][1]) + (pa[k][0] * pa[i][1])) + (pa[i][0] * pa[j][1])) - (pa[k][0] * pa[j][1])) + + (-pa[i][0] * pa[k][1])) + (pa[1][j] * pa[k][1])); i = 1; j = 2, k = 3; - double area123 = ((-pa[j][0] * pa[i][1]) + (pa[k][0] * pa[i][1]) + (pa[i][0] * pa[j][1]) - (pa[k][0] * pa[j][1]) + - (-pa[i][0] * pa[k][1]) + (pa[1][j] * pa[k][1])); + double area123 = ((((((-pa[j][0] * pa[i][1]) + (pa[k][0] * pa[i][1])) + (pa[i][0] * pa[j][1])) - (pa[k][0] * pa[j][1])) + + (-pa[i][0] * pa[k][1])) + (pa[1][j] * pa[k][1])); double sum_area = area012 + area013 + area023 + area123; - return ((sum_area < threshold_area) || - (iscolinear(pa[0], pa[1], pa[2]) || iscolinear(pa[0], pa[1], pa[3]) || iscolinear(pa[0], pa[2], pa[3]) || - iscolinear(pa[1], pa[2], pa[3]) || iscolinear(pb[0], pb[1], pb[2]) || iscolinear(pb[0], pb[1], pb[3]) || - iscolinear(pb[0], pb[2], pb[3]) || iscolinear(pb[1], pb[2], pb[3]))); + if (sum_area < threshold_area) { + return true; + } + else if (iscolinear(pa[0], pa[1], pa[2])) { + return true; + } + else if (iscolinear(pa[0], pa[1], pa[3])) { + return true; + } + else if (iscolinear(pa[0], pa[2], pa[3])) { + return true; + } + else if (iscolinear(pa[1], pa[2], pa[3])) { + return true; + } + else if (iscolinear(pb[0], pb[1], pb[2])) { + return true; + } + else if (iscolinear(pb[0], pb[1], pb[3])) { + return true; + } + else if (iscolinear(pb[0], pb[2], pb[3])) { + return true; + } + else if (iscolinear(pb[1], pb[2], pb[3])) { + return true; + } + else { + return false; + } } + /* -\brief -Function to determine if a set of 4 pairs of matched points give rise -to a degeneracy in the calculation of a homography as needed by RANSAC. -This involves testing whether any 3 of the 4 points in each set is -colinear. - -point are coded this way -x1b,y1b, x2b, y2b, ... xnb, ynb -x1a,y1a, x2a, y2a, ... xna, yna -leading to 2*2*n + \brief + Function to determine if a set of 4 pairs of matched points give rise + to a degeneracy in the calculation of a homography as needed by RANSAC. + This involves testing whether any 3 of the 4 points in each set is + colinear. + + point are coded this way + x1b,y1b, x2b, y2b, ... xnb, ynb + x1a,y1a, x2a, y2a, ... xna, yna + leading to 2*2*n */ bool vpHomography::degenerateConfiguration(const vpColVector &x, unsigned int *ind) { @@ -156,10 +183,36 @@ bool vpHomography::degenerateConfiguration(const vpColVector &x, unsigned int *i pa[i][1] = x[n2 + ind2 + 1]; pa[i][2] = 1; } - return (iscolinear(pa[0], pa[1], pa[2]) || iscolinear(pa[0], pa[1], pa[3]) || iscolinear(pa[0], pa[2], pa[3]) || - iscolinear(pa[1], pa[2], pa[3]) || iscolinear(pb[0], pb[1], pb[2]) || iscolinear(pb[0], pb[1], pb[3]) || - iscolinear(pb[0], pb[2], pb[3]) || iscolinear(pb[1], pb[2], pb[3])); + + if (iscolinear(pa[0], pa[1], pa[2])) { + return true; + } + else if (iscolinear(pa[0], pa[1], pa[3])) { + return true; + } + else if (iscolinear(pa[0], pa[2], pa[3])) { + return true; + } + else if (iscolinear(pa[1], pa[2], pa[3])) { + return true; + } + else if (iscolinear(pb[0], pb[1], pb[2])) { + return true; + } + else if (iscolinear(pb[0], pb[1], pb[3])) { + return true; + } + else if (iscolinear(pb[0], pb[2], pb[3])) { + return true; + } + else if (iscolinear(pb[1], pb[2], pb[3])) { + return true; + } + else { + return false; + } } + bool vpHomography::degenerateConfiguration(const std::vector &xb, const std::vector &yb, const std::vector &xa, const std::vector &ya) { diff --git a/modules/vision/src/homography-estimation/vpHomographyVVS.cpp b/modules/vision/src/homography-estimation/vpHomographyVVS.cpp index d095bc0739..99e53f0fb4 100644 --- a/modules/vision/src/homography-estimation/vpHomographyVVS.cpp +++ b/modules/vision/src/homography-estimation/vpHomographyVVS.cpp @@ -69,7 +69,8 @@ static void updatePoseRotation(vpColVector &dx, vpHomogeneousMatrix &mati) rd[2][0] = (-sinu * u[1]) + (mcosi * u[2] * u[0]); rd[2][1] = (sinu * u[0]) + (mcosi * u[2] * u[1]); rd[2][2] = cosi + (mcosi * u[2] * u[2]); - } else { + } + else { for (unsigned int i = 0; i < 3; ++i) { for (unsigned int j = 0; j < 3; ++j) { rd[i][j] = 0.0; @@ -125,7 +126,8 @@ double vpHomography::computeRotation(unsigned int nbpoint, vpPoint *c1P, vpPoint W = 0; vpMatrix c2Rc1(3, 3); double r = 0; - while (vpMath::equal(r_1, r, m_threshold_rotation) == false) { + bool stop = false; + while ((vpMath::equal(r_1, r, m_threshold_rotation) == false) && (stop == false)) { r_1 = r; // compute current position @@ -191,23 +193,28 @@ double vpHomography::computeRotation(unsigned int nbpoint, vpPoint *c1P, vpPoint if (k == 0) { L = H2; e = e2; - } else { + } + else { L = vpMatrix::stack(L, H2); e = vpColVector::stack(e, e2); } - } else if (only_1) { + } + else if (only_1) { if (k == 0) { L = H1; e = e1; - } else { + } + else { L = vpMatrix::stack(L, H1); e = vpColVector::stack(e, e1); } - } else { + } + else { if (k == 0) { L = H2; e = e2; - } else { + } + else { L = vpMatrix::stack(L, H2); e = vpColVector::stack(e, e2); } @@ -221,7 +228,7 @@ double vpHomography::computeRotation(unsigned int nbpoint, vpPoint *c1P, vpPoint if (userobust) { for (unsigned int l = 0; l < n; ++l) { - res[l] = vpMath::sqr(e[2 * l]) + vpMath::sqr(e[(2 * l )+ 1]); + res[l] = vpMath::sqr(e[2 * l]) + vpMath::sqr(e[(2 * l)+ 1]); } robust.MEstimator(vpRobust::TUKEY, res, w); @@ -230,7 +237,8 @@ double vpHomography::computeRotation(unsigned int nbpoint, vpPoint *c1P, vpPoint W[2 * l][2 * l] = w[l]; W[(2 * l) + 1][(2 * l) + 1] = w[l]; } - } else { + } + else { for (unsigned int l = 0; l < (2 * n); ++l) { W[l][l] = 1; } @@ -250,7 +258,7 @@ double vpHomography::computeRotation(unsigned int nbpoint, vpPoint *c1P, vpPoint r = e.sumSquare(); if (((W * e).sumSquare() < 1e-10) || (iter > 25)) { - break; + stop = true; } ++iter; } @@ -307,7 +315,9 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP double r = 1e10; iter = 0; - while (!vpMath::equal(r_1, r, m_threshold_displacement)) { + vpMatrix sTR; + bool stop = false; + while ((!vpMath::equal(r_1, r, m_threshold_displacement)) && (stop == false)) { r_1 = r; // compute current position @@ -366,16 +376,16 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP H2 *= -1; vpMatrix c1CFc2(6, 6); - { - vpMatrix sTR = c1Tc2.skew() * static_cast(c1Rc2); - for (unsigned int k_ = 0; k_ < 3; ++k_) { - for (unsigned int l = 0; l < 3; ++l) { - c1CFc2[k_][l] = c1Rc2[k_][l]; - c1CFc2[k_ + 3][l + 3] = c1Rc2[k_][l]; - c1CFc2[k_][l + 3] = sTR[k_][l]; - } + + sTR = c1Tc2.skew() * c1Rc2; + for (unsigned int k_ = 0; k_ < 3; ++k_) { + for (unsigned int l = 0; l < 3; ++l) { + c1CFc2[k_][l] = c1Rc2[k_][l]; + c1CFc2[k_ + 3][l + 3] = c1Rc2[k_][l]; + c1CFc2[k_][l + 3] = sTR[k_][l]; } } + H2 = H2 * c1CFc2; // Set up the error vector @@ -408,23 +418,28 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP if (k == 0) { L = H2; e = e2; - } else { + } + else { L = vpMatrix::stack(L, H2); e = vpColVector::stack(e, e2); } - } else if (only_1) { + } + else if (only_1) { if (k == 0) { L = H1; e = e1; - } else { + } + else { L = vpMatrix::stack(L, H1); e = vpColVector::stack(e, e1); } - } else { + } + else { if (k == 0) { L = H2; e = e2; - } else { + } + else { L = vpMatrix::stack(L, H2); e = vpColVector::stack(e, e2); } @@ -437,7 +452,7 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP if (userobust) { for (unsigned int l = 0; l < n; ++l) { - res[l] = vpMath::sqr(e[2 * l]) + vpMath::sqr(e[(2 * l )+ 1]); + res[l] = vpMath::sqr(e[2 * l]) + vpMath::sqr(e[(2 * l)+ 1]); } robust.MEstimator(vpRobust::TUKEY, res, w); @@ -446,7 +461,8 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP W[2 * l][2 * l] = w[l]; W[(2 * l) + 1][(2 * l) + 1] = w[l]; } - } else { + } + else { for (unsigned int l = 0; l < (2 * n); ++l) { W[l][l] = 1; } @@ -463,7 +479,7 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP r = (W * e).sumSquare(); if ((r < 1e-15) || (iter > 1000) || (r > r_1)) { - break; + stop = true; } ++iter; } @@ -508,7 +524,9 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP double r = 1e10; iter = 0; - while (!vpMath::equal(r_1, r, m_threshold_displacement)) { + vpMatrix sTR; + bool stop = false; + while ((!vpMath::equal(r_1, r, m_threshold_displacement)) && (stop == false)) { r_1 = r; // compute current position @@ -566,16 +584,16 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP H2 *= -1; vpMatrix c1CFc2(6, 6); - { - vpMatrix sTR = c1Tc2.skew() * static_cast(c1Rc2); - for (unsigned int k_ = 0; k_ < 3; ++k_) { - for (unsigned int l = 0; l < 3; ++l) { - c1CFc2[k_][l] = c1Rc2[k_][l]; - c1CFc2[k_ + 3][l + 3] = c1Rc2[k_][l]; - c1CFc2[k_][l + 3] = sTR[k_][l]; - } + + sTR = c1Tc2.skew() * static_cast(c1Rc2); + for (unsigned int k_ = 0; k_ < 3; ++k_) { + for (unsigned int l = 0; l < 3; ++l) { + c1CFc2[k_][l] = c1Rc2[k_][l]; + c1CFc2[k_ + 3][l + 3] = c1Rc2[k_][l]; + c1CFc2[k_][l + 3] = sTR[k_][l]; } } + H2 = H2 * c1CFc2; // Set up the error vector @@ -608,23 +626,28 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP if (k == 0) { L = H2; e = e2; - } else { + } + else { L = vpMatrix::stack(L, H2); e = vpColVector::stack(e, e2); } - } else if (only_1) { + } + else if (only_1) { if (k == 0) { L = H1; e = e1; - } else { + } + else { L = vpMatrix::stack(L, H1); e = vpColVector::stack(e, e1); } - } else { + } + else { if (k == 0) { L = H2; e = e2; - } else { + } + else { L = vpMatrix::stack(L, H2); e = vpColVector::stack(e, e2); } @@ -646,7 +669,8 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP W[2 * k_][2 * k_] = w[k_]; W[(2 * k_) + 1][(2 * k_) + 1] = w[k_]; } - } else { + } + else { for (unsigned int k_ = 0; k_ < (2 * n); ++k_) { W[k_][k_] = 1; } @@ -663,7 +687,7 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP r = (W * e).sumSquare(); if ((r < 1e-15) || (iter > 1000) || (r > r_1)) { - break; + stop = true; } ++iter; } From ed78d689c505dd7ea4416a86b137ff205ebc361f Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Wed, 10 Apr 2024 14:37:40 +0200 Subject: [PATCH 15/17] Introduce new quality rules fixes --- modules/core/include/visp3/core/vpArray2D.h | 60 +- modules/core/include/visp3/core/vpColVector.h | 30 +- modules/core/include/visp3/core/vpDisplay.h | 2 +- modules/core/include/visp3/core/vpGEMM.h | 90 +- modules/core/include/visp3/core/vpHistogram.h | 10 +- .../include/visp3/core/vpHomogeneousMatrix.h | 4 +- modules/core/include/visp3/core/vpImage.h | 106 +-- .../include/visp3/core/vpImageMorphology.h | 44 +- .../core/include/visp3/core/vpImageTools.h | 104 +-- modules/core/include/visp3/core/vpIoTools.h | 10 +- modules/core/include/visp3/core/vpMath.h | 2 +- modules/core/include/visp3/core/vpMatrix.h | 4 +- .../visp3/core/vpPixelMeterConversion.h | 4 +- modules/core/include/visp3/core/vpRansac.h | 7 +- .../include/visp3/core/vpRotationMatrix.h | 4 +- .../include/visp3/core/vpRotationVector.h | 2 +- modules/core/include/visp3/core/vpRowVector.h | 4 +- .../core/src/camera/vpCameraParameters.cpp | 86 +- .../src/camera/vpColorDepthConversion.cpp | 6 +- .../src/camera/vpMeterPixelConversion.cpp | 2 +- .../src/camera/vpPixelMeterConversion.cpp | 52 +- modules/core/src/camera/vpXmlParserCamera.cpp | 2 +- modules/core/src/display/vpDisplay.cpp | 12 +- modules/core/src/display/vpDisplay_impl.h | 12 +- modules/core/src/display/vpDisplay_rgba.cpp | 40 +- modules/core/src/display/vpDisplay_uchar.cpp | 36 +- modules/core/src/display/vpFeatureDisplay.cpp | 16 +- .../src/image/private/vpBayerConversion.h | 128 +-- .../src/image/private/vpImageConvert_impl.h | 12 +- .../core/src/image/vpCannyEdgeDetection.cpp | 57 +- modules/core/src/image/vpImageCircle.cpp | 184 ++-- modules/core/src/image/vpImageConvert.cpp | 18 +- modules/core/src/image/vpImageFilter.cpp | 14 +- modules/core/src/image/vpImagePoint.cpp | 22 +- modules/core/src/image/vpImageTools.cpp | 40 +- modules/core/src/image/vpRGBa.cpp | 30 +- modules/core/src/image/vpRGBf.cpp | 35 +- modules/core/src/math/matrix/vpColVector.cpp | 210 +++-- .../src/math/matrix/vpEigenConversion.cpp | 18 +- modules/core/src/math/matrix/vpMatrix.cpp | 633 ++++++++------ .../src/math/matrix/vpMatrix_cholesky.cpp | 4 +- .../src/math/matrix/vpMatrix_covariance.cpp | 46 +- modules/core/src/math/matrix/vpMatrix_lu.cpp | 56 +- modules/core/src/math/matrix/vpMatrix_qr.cpp | 56 +- modules/core/src/math/matrix/vpRowVector.cpp | 180 ++-- .../core/src/math/matrix/vpSubColVector.cpp | 19 +- modules/core/src/math/matrix/vpSubMatrix.cpp | 27 +- .../core/src/math/matrix/vpSubRowVector.cpp | 20 +- modules/core/src/math/misc/vpMath.cpp | 20 +- .../src/math/random-generator/vpUniRand.cpp | 4 +- modules/core/src/math/robust/vpRobust.cpp | 47 +- .../math/transformation/vpExponentialMap.cpp | 133 +-- .../transformation/vpForceTwistMatrix.cpp | 95 ++- .../transformation/vpHomogeneousMatrix.cpp | 118 +-- .../src/math/transformation/vpPoseVector.cpp | 37 +- .../transformation/vpQuaternionVector.cpp | 35 +- .../math/transformation/vpRotationMatrix.cpp | 184 ++-- .../math/transformation/vpRotationVector.cpp | 12 +- .../src/math/transformation/vpRxyzVector.cpp | 22 +- .../src/math/transformation/vpRzyxVector.cpp | 16 +- .../src/math/transformation/vpRzyzVector.cpp | 22 +- .../math/transformation/vpThetaUVector.cpp | 81 +- .../transformation/vpTranslationVector.cpp | 41 +- .../transformation/vpVelocityTwistMatrix.cpp | 113 +-- .../vpXmlParserHomogeneousMatrix.cpp | 27 +- modules/core/src/tools/file/basisu_miniz.h | 801 ++++++++---------- modules/core/src/tools/file/vpIoTools.cpp | 18 +- modules/core/src/tools/geometry/vpPlane.cpp | 88 +- modules/core/src/tools/geometry/vpPolygon.cpp | 59 +- modules/core/src/tools/geometry/vpRect.cpp | 58 +- .../src/tools/geometry/vpRectOriented.cpp | 103 +-- .../geometry/vpXmlParserRectOriented.cpp | 3 +- .../core/src/tools/histogram/vpHistogram.cpp | 179 ++-- .../src/tools/histogram/vpHistogramPeak.cpp | 2 +- .../src/tools/histogram/vpHistogramValey.cpp | 2 +- modules/core/src/tools/time/vpTime.cpp | 12 +- .../tracking/forward-projection/vpCircle.cpp | 128 +-- .../tracking/forward-projection/vpLine.cpp | 127 ++- .../tracking/forward-projection/vpPoint.cpp | 76 +- .../tracking/forward-projection/vpSphere.cpp | 38 +- .../visp3/detection/vpDetectorAprilTag.h | 8 +- .../detection/src/tag/vpDetectorAprilTag.cpp | 8 +- modules/gui/include/visp3/gui/vpDisplayGDI.h | 2 +- modules/gui/include/visp3/gui/vpDisplayGTK.h | 2 +- .../gui/include/visp3/gui/vpDisplayOpenCV.h | 2 +- modules/gui/include/visp3/gui/vpDisplayX.h | 2 +- modules/gui/include/visp3/gui/vpPlot.h | 2 +- modules/gui/src/display/vpDisplayGTK.cpp | 4 +- .../gui/src/display/windows/vpD3DRenderer.cpp | 38 +- .../gui/src/display/windows/vpGDIRenderer.cpp | 24 +- modules/gui/src/plot/vpPlot.cpp | 16 +- modules/gui/src/plot/vpPlotGraph.cpp | 14 +- modules/imgproc/src/vpCLAHE.cpp | 135 +-- .../imgproc/src/vpCircleHoughTransform.cpp | 49 +- modules/imgproc/src/vpConnectedComponents.cpp | 31 +- modules/imgproc/src/vpContours.cpp | 67 +- modules/imgproc/src/vpFloodFill.cpp | 28 +- modules/imgproc/src/vpImgproc.cpp | 70 +- modules/imgproc/src/vpMorph.cpp | 26 +- modules/imgproc/src/vpRetinex.cpp | 78 +- modules/imgproc/src/vpThreshold.cpp | 131 +-- .../io/src/image/private/vpImageIoLibjpeg.cpp | 24 +- .../io/src/image/private/vpImageIoLibpng.cpp | 76 +- .../src/image/private/vpImageIoPortable.cpp | 36 +- .../io/src/image/private/vpImageIoSimd.cpp | 4 +- modules/io/src/image/private/vpImageIoStb.cpp | 2 +- .../io/src/image/private/vpImageIoTinyEXR.cpp | 34 +- modules/io/src/tools/vpParseArgv.cpp | 32 +- modules/io/src/video/vpVideoReader.cpp | 6 +- modules/tracker/blob/src/dots/vpDot.cpp | 16 +- modules/tracker/blob/src/dots/vpDot2.cpp | 510 +++++------ .../include/visp3/vision/vpHomography.h | 6 +- .../private/vpLevenbergMarquartd.cpp | 415 +++++---- modules/vision/src/pose-estimation/vpPose.cpp | 4 +- .../src/pose-estimation/vpPoseDementhon.cpp | 34 +- .../src/pose-estimation/vpPoseLagrange.cpp | 44 +- .../vision/src/pose-estimation/vpPoseLowe.cpp | 16 +- .../vision/src/pose-estimation/vpPoseRGBD.cpp | 46 +- .../src/pose-estimation/vpPoseRansac.cpp | 22 +- .../vpPoseVirtualVisualServoing.cpp | 8 +- 120 files changed, 3893 insertions(+), 3440 deletions(-) diff --git a/modules/core/include/visp3/core/vpArray2D.h b/modules/core/include/visp3/core/vpArray2D.h index 753444f346..8576bc4d61 100644 --- a/modules/core/include/visp3/core/vpArray2D.h +++ b/modules/core/include/visp3/core/vpArray2D.h @@ -78,8 +78,8 @@ * a[1][0] = 4; a[1][1] = 5.5; a[1][2] = 6; * * std::cout << "a:" << std::endl; - * for (unsigned int i = 0; i < a.getRows(); i++) { - * for (unsigned int j = 0; j < a.getCols(); j++) { + * for (unsigned int i = 0; i < a.getRows(); ++i) { + * for (unsigned int j = 0; j < a.getCols(); ++j) { * std::cout << a[i][j] << " "; * } * std::cout << std::endl; @@ -475,7 +475,7 @@ template class vpArray2D memcpy(data + r * colNum, A.data, sizeof(Type) * A.size()); } else if (data != nullptr && A.data != nullptr && A.data != data) { - for (unsigned int i = r; i < (r + A.getRows()); i++) { + for (unsigned int i = r; i < (r + A.getRows()); ++i) { memcpy(data + i * colNum + c, A.data + (i - r) * A.colNum, sizeof(Type) * A.colNum); } } @@ -592,8 +592,8 @@ template class vpArray2D std::ios_base::fmtflags original_flags = s.flags(); s.precision(10); - for (unsigned int i = 0; i < A.getRows(); i++) { - for (unsigned int j = 0; j < A.getCols() - 1; j++) { + for (unsigned int i = 0; i < A.getRows(); ++i) { + for (unsigned int j = 0; j < A.getCols() - 1; ++j) { s << A[i][j] << " "; } // We don't add " " after the last row element @@ -698,8 +698,8 @@ template class vpArray2D A.resize(rows, cols); Type value; - for (unsigned int i = 0; i < rows; i++) { - for (unsigned int j = 0; j < cols; j++) { + for (unsigned int i = 0; i < rows; ++i) { + for (unsigned int j = 0; j < cols; ++j) { file >> value; A[i][j] = value; } @@ -728,8 +728,8 @@ template class vpArray2D A.resize(rows, cols); Type value; - for (unsigned int i = 0; i < rows; i++) { - for (unsigned int j = 0; j < cols; j++) { + for (unsigned int i = 0; i < rows; ++i) { + for (unsigned int j = 0; j < cols; ++j) { file.read((char *)&value, sizeof(Type)); A[i][j] = value; } @@ -881,8 +881,8 @@ template class vpArray2D matrixSize = A.getCols(); file.write((char *)&matrixSize, sizeof(unsigned int)); Type value; - for (unsigned int i = 0; i < A.getRows(); i++) { - for (unsigned int j = 0; j < A.getCols(); j++) { + for (unsigned int i = 0; i < A.getRows(); ++i) { + for (unsigned int j = 0; j < A.getCols(); ++j) { value = A[i][j]; file.write((char *)&value, sizeof(Type)); } @@ -1068,7 +1068,7 @@ template Type vpArray2D::getMinValue() const Type *dataptr = data; Type min = *dataptr; dataptr++; - for (unsigned int i = 0; i < dsize - 1; i++) { + for (unsigned int i = 0; i < dsize - 1; ++i) { if (*dataptr < min) { min = *dataptr; } @@ -1085,7 +1085,7 @@ template Type vpArray2D::getMaxValue() const Type *dataptr = data; Type max = *dataptr; dataptr++; - for (unsigned int i = 0; i < dsize - 1; i++) { + for (unsigned int i = 0; i < dsize - 1; ++i) { if (*dataptr > max) { max = *dataptr; } @@ -1109,7 +1109,7 @@ template vpArray2D vpArray2D::hadamard(const vpArray2D< vpArray2D out; out.resize(rowNum, colNum, false); - for (unsigned int i = 0; i < dsize; i++) { + for (unsigned int i = 0; i < dsize; ++i) { out.data[i] = data[i] * m.data[i]; } @@ -1119,8 +1119,8 @@ template vpArray2D vpArray2D::hadamard(const vpArray2D< template vpArray2D vpArray2D::t() const { vpArray2D At(colNum, rowNum); - for (unsigned int i = 0; i < rowNum; i++) { - for (unsigned int j = 0; j < colNum; j++) { + for (unsigned int i = 0; i < rowNum; ++i) { + for (unsigned int j = 0; j < colNum; ++j) { At[j][i] = (*this)[i][j]; } } @@ -1169,10 +1169,10 @@ template void vpArray2D::conv2(const vpArray2D &M, cons } if (mode == "same") { - for (unsigned int i = 0; i < res_same.getRows(); i++) { - for (unsigned int j = 0; j < res_same.getCols(); j++) { - for (unsigned int k = 0; k < kernel.getRows(); k++) { - for (unsigned int l = 0; l < kernel.getCols(); l++) { + for (unsigned int i = 0; i < res_same.getRows(); ++i) { + for (unsigned int j = 0; j < res_same.getCols(); ++j) { + for (unsigned int k = 0; k < kernel.getRows(); ++k) { + for (unsigned int l = 0; l < kernel.getCols(); ++l) { res_same[i][j] += M_padded[i + k][j + l] * kernel[kernel.getRows() - k - 1][kernel.getCols() - l - 1]; } } @@ -1181,16 +1181,16 @@ template void vpArray2D::conv2(const vpArray2D &M, cons const unsigned int start_i = kernel.getRows() / 2; const unsigned int start_j = kernel.getCols() / 2; - for (unsigned int i = 0; i < M.getRows(); i++) { + for (unsigned int i = 0; i < M.getRows(); ++i) { memcpy(res.data + i * M.getCols(), res_same.data + (i + start_i) * res_same.getCols() + start_j, sizeof(Type) * M.getCols()); } } else { - for (unsigned int i = 0; i < res.getRows(); i++) { - for (unsigned int j = 0; j < res.getCols(); j++) { - for (unsigned int k = 0; k < kernel.getRows(); k++) { - for (unsigned int l = 0; l < kernel.getCols(); l++) { + for (unsigned int i = 0; i < res.getRows(); ++i) { + for (unsigned int j = 0; j < res.getCols(); ++j) { + for (unsigned int k = 0; k < kernel.getRows(); ++k) { + for (unsigned int l = 0; l < kernel.getCols(); ++l) { res[i][j] += M_padded[i + k][j + l] * kernel[kernel.getRows() - k - 1][kernel.getCols() - l - 1]; } } @@ -1213,8 +1213,8 @@ template void vpArray2D::insert(const vpArray2D &A, cons if (((r + B.getRows()) <= A.getRows()) && ((c + B.getCols()) <= A.getCols())) { C.resize(A.getRows(), A.getCols(), false, false); - for (unsigned int i = 0; i < A.getRows(); i++) { - for (unsigned int j = 0; j < A.getCols(); j++) { + for (unsigned int i = 0; i < A.getRows(); ++i) { + for (unsigned int j = 0; j < A.getCols(); ++j) { if (i >= r && i < (r + B.getRows()) && j >= c && j < (c + B.getCols())) { C[i][j] = B[i - r][j - c]; } @@ -1236,7 +1236,7 @@ template bool vpArray2D::operator==(const vpArray2D &A) return false; } - for (unsigned int i = 0; i < A.size(); i++) { + for (unsigned int i = 0; i < A.size(); ++i) { if (data[i] != A.data[i]) { return false; } @@ -1254,7 +1254,7 @@ template <> inline bool vpArray2D::operator==(const vpArray2D &A return false; } - for (unsigned int i = 0; i < A.size(); i++) { + for (unsigned int i = 0; i < A.size(); ++i) { if (fabs(data[i] - A.data[i]) > std::numeric_limits::epsilon()) { return false; } @@ -1272,7 +1272,7 @@ template <> inline bool vpArray2D::operator==(const vpArray2D &A) return false; } - for (unsigned int i = 0; i < A.size(); i++) { + for (unsigned int i = 0; i < A.size(); ++i) { if (fabsf(data[i] - A.data[i]) > std::numeric_limits::epsilon()) { return false; } diff --git a/modules/core/include/visp3/core/vpColVector.h b/modules/core/include/visp3/core/vpColVector.h index 435d5abea0..e1e2ac4420 100644 --- a/modules/core/include/visp3/core/vpColVector.h +++ b/modules/core/include/visp3/core/vpColVector.h @@ -74,7 +74,7 @@ class vpPoseVector; * v[0] = -1; v[1] = -2.1; v[2] = -3; * * std::cout << "v:" << std::endl; - * for (unsigned int i = 0; i < v.size(); i++) { + * for (unsigned int i = 0; i < v.size(); ++i) { * std::cout << v[i] << std::endl; * } * } @@ -288,7 +288,7 @@ class VISP_EXPORT vpColVector : public vpArray2D * int main() * { * vpColVector v(3); - * for (unsigned int i=0; i * { * std::ofstream ofs("log.csv", std::ofstream::out); * vpColVector v(3); - * for (unsigned int i=0; i * * \code * vpColVector v1; - * for (unsigned int i=0; i<4; i++) + * for (unsigned int i=0; i<4; ++i) * v1.stack(i); * // v1 is equal to [0 1 2 3]^T * vpColVector v2 = v1.extract(1, 3); @@ -429,7 +429,7 @@ class VISP_EXPORT vpColVector : public vpArray2D * { * vpColVector v(4); * int val = 0; - * for(size_t i=0; i * int main() * { * vpColVector v(4); - * for (unsigned int i=0; i < v.size(); i++) + * for (unsigned int i=0; i < v.size(); ++i) * v[i] = i; * std::cout << "v: " << v.t() << std::endl; * * vpColVector w(2); - * for (unsigned int i=0; i < w.size(); i++) + * for (unsigned int i=0; i < w.size(); ++i) * w[i] = i+10; * std::cout << "w: " << w.t() << std::endl; * @@ -491,7 +491,7 @@ class VISP_EXPORT vpColVector : public vpArray2D * int main() * { * vpColVector v(3); - * for (unsigned int i=0; i * size_t n = 5; * vpColVector A(n); * double *B = new double [n]; - * for (unsigned int i = 0; i < n; i++) + * for (unsigned int i = 0; i < n; ++i) * B[i] = i; * A << B; * std::cout << "A: " << A.t() << std::endl; @@ -993,8 +993,8 @@ class VISP_EXPORT vpColVector : public vpArray2D * { * int var=0; * vpMatrix mat(3, 4); - * for (int i = 0; i < 3; i++) - * for (int j = 0; j < 4; j++) + * for (int i = 0; i < 3; ++i) + * for (int j = 0; j < 4; ++j) * mat[i][j] = ++var; * std::cout << "mat: \n" << mat << std::endl; * @@ -1369,12 +1369,12 @@ class VISP_EXPORT vpColVector : public vpArray2D * int main() * { * vpColVector v(4); - * for (unsigned int i=0; i < v.size(); i++) + * for (unsigned int i=0; i < v.size(); ++i) * v[i] = i; * std::cout << "v: " << v.t() << std::endl; * * vpColVector w(2); - * for (unsigned int i=0; i < w.size(); i++) + * for (unsigned int i=0; i < w.size(); ++i) * w[i] = i+10; * std::cout << "w: " << w.t() << std::endl; * diff --git a/modules/core/include/visp3/core/vpDisplay.h b/modules/core/include/visp3/core/vpDisplay.h index d784891c53..822256aaf2 100644 --- a/modules/core/include/visp3/core/vpDisplay.h +++ b/modules/core/include/visp3/core/vpDisplay.h @@ -143,7 +143,7 @@ * std::cout << "Check keyboard events..." << std::endl; * char key[10]; sprintf(key, "\0"); * bool ret; - * for (int i=0; i< 200; i++) { + * for (int i=0; i< 200; ++i) { * bool ret = vpDisplay::getKeyboardEvent(I, key, false); * if (ret) * std::cout << "keyboard event: key: " << "\"" << key diff --git a/modules/core/include/visp3/core/vpGEMM.h b/modules/core/include/visp3/core/vpGEMM.h index 4e6a9e4ba6..ad0b44c0ea 100644 --- a/modules/core/include/visp3/core/vpGEMM.h +++ b/modules/core/include/visp3/core/vpGEMM.h @@ -49,7 +49,8 @@ const vpArray2D null(0, 0); \relates vpArray2D */ -typedef enum { +typedef enum +{ VP_GEMM_A_T = 1, //! Use A^T instead of A VP_GEMM_B_T = 2, //! Use B^T instead of B VP_GEMM_C_T = 4, //! Use C^T instead of C @@ -58,8 +59,7 @@ typedef enum { template inline void GEMMsize(const vpArray2D & /*A*/, const vpArray2D & /*B*/, unsigned int & /*Arows*/, unsigned int & /*Acols*/, unsigned int & /*Brows*/, unsigned int & /*Bcols*/) -{ -} +{ } template <> void inline GEMMsize<0>(const vpArray2D &A, const vpArray2D &B, unsigned int &Arows, @@ -143,17 +143,16 @@ template inline void GEMM1(const unsigned int & /*Arows*/, const unsigned int & /*Brows*/, const unsigned int & /*Bcols*/, const vpArray2D & /*A*/, const vpArray2D & /*B*/, const double & /*alpha*/, vpArray2D & /*D*/) -{ -} +{ } template <> inline void GEMM1<0>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols, const vpArray2D &A, const vpArray2D &B, const double &alpha, vpArray2D &D) { - for (unsigned int r = 0; r < Arows; r++) - for (unsigned int c = 0; c < Bcols; c++) { + for (unsigned int r = 0; r < Arows; ++r) + for (unsigned int c = 0; c < Bcols; ++c) { double sum = 0; - for (unsigned int n = 0; n < Brows; n++) + for (unsigned int n = 0; n < Brows; ++n) sum += A[r][n] * B[n][c] * alpha; D[r][c] = sum; } @@ -163,10 +162,10 @@ template <> inline void GEMM1<1>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols, const vpArray2D &A, const vpArray2D &B, const double &alpha, vpArray2D &D) { - for (unsigned int r = 0; r < Arows; r++) - for (unsigned int c = 0; c < Bcols; c++) { + for (unsigned int r = 0; r < Arows; ++r) + for (unsigned int c = 0; c < Bcols; ++c) { double sum = 0; - for (unsigned int n = 0; n < Brows; n++) + for (unsigned int n = 0; n < Brows; ++n) sum += A[n][r] * B[n][c] * alpha; D[r][c] = sum; } @@ -176,10 +175,10 @@ template <> inline void GEMM1<2>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols, const vpArray2D &A, const vpArray2D &B, const double &alpha, vpArray2D &D) { - for (unsigned int r = 0; r < Arows; r++) - for (unsigned int c = 0; c < Bcols; c++) { + for (unsigned int r = 0; r < Arows; ++r) + for (unsigned int c = 0; c < Bcols; ++c) { double sum = 0; - for (unsigned int n = 0; n < Brows; n++) + for (unsigned int n = 0; n < Brows; ++n) sum += A[r][n] * B[c][n] * alpha; D[r][c] = sum; } @@ -189,10 +188,10 @@ template <> inline void GEMM1<3>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols, const vpArray2D &A, const vpArray2D &B, const double &alpha, vpArray2D &D) { - for (unsigned int r = 0; r < Arows; r++) - for (unsigned int c = 0; c < Bcols; c++) { + for (unsigned int r = 0; r < Arows; ++r) + for (unsigned int c = 0; c < Bcols; ++c) { double sum = 0; - for (unsigned int n = 0; n < Brows; n++) + for (unsigned int n = 0; n < Brows; ++n) sum += A[n][r] * B[c][n] * alpha; D[r][c] = sum; } @@ -202,18 +201,17 @@ template inline void GEMM2(const unsigned int & /*Arows*/, const unsigned int & /*Brows*/, const unsigned int & /*Bcols*/, const vpArray2D & /*A*/, const vpArray2D & /*B*/, const double & /*alpha*/, const vpArray2D & /*C*/, const double & /*beta*/, vpArray2D & /*D*/) -{ -} +{ } template <> inline void GEMM2<0>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols, const vpArray2D &A, const vpArray2D &B, const double &alpha, const vpArray2D &C, const double &beta, vpArray2D &D) { - for (unsigned int r = 0; r < Arows; r++) - for (unsigned int c = 0; c < Bcols; c++) { + for (unsigned int r = 0; r < Arows; ++r) + for (unsigned int c = 0; c < Bcols; ++c) { double sum = 0; - for (unsigned int n = 0; n < Brows; n++) + for (unsigned int n = 0; n < Brows; ++n) sum += A[r][n] * B[n][c] * alpha; D[r][c] = sum + C[r][c] * beta; } @@ -224,10 +222,10 @@ inline void GEMM2<1>(const unsigned int &Arows, const unsigned int &Brows, const const vpArray2D &A, const vpArray2D &B, const double &alpha, const vpArray2D &C, const double &beta, vpArray2D &D) { - for (unsigned int r = 0; r < Arows; r++) - for (unsigned int c = 0; c < Bcols; c++) { + for (unsigned int r = 0; r < Arows; ++r) + for (unsigned int c = 0; c < Bcols; ++c) { double sum = 0; - for (unsigned int n = 0; n < Brows; n++) + for (unsigned int n = 0; n < Brows; ++n) sum += A[n][r] * B[n][c] * alpha; D[r][c] = sum + C[r][c] * beta; } @@ -238,10 +236,10 @@ inline void GEMM2<2>(const unsigned int &Arows, const unsigned int &Brows, const const vpArray2D &A, const vpArray2D &B, const double &alpha, const vpArray2D &C, const double &beta, vpArray2D &D) { - for (unsigned int r = 0; r < Arows; r++) - for (unsigned int c = 0; c < Bcols; c++) { + for (unsigned int r = 0; r < Arows; ++r) + for (unsigned int c = 0; c < Bcols; ++c) { double sum = 0; - for (unsigned int n = 0; n < Brows; n++) + for (unsigned int n = 0; n < Brows; ++n) sum += A[r][n] * B[c][n] * alpha; D[r][c] = sum + C[r][c] * beta; } @@ -252,10 +250,10 @@ inline void GEMM2<3>(const unsigned int &Arows, const unsigned int &Brows, const const vpArray2D &A, const vpArray2D &B, const double &alpha, const vpArray2D &C, const double &beta, vpArray2D &D) { - for (unsigned int r = 0; r < Arows; r++) - for (unsigned int c = 0; c < Bcols; c++) { + for (unsigned int r = 0; r < Arows; ++r) + for (unsigned int c = 0; c < Bcols; ++c) { double sum = 0; - for (unsigned int n = 0; n < Brows; n++) + for (unsigned int n = 0; n < Brows; ++n) sum += A[n][r] * B[c][n] * alpha; D[r][c] = sum + C[r][c] * beta; } @@ -266,10 +264,10 @@ inline void GEMM2<4>(const unsigned int &Arows, const unsigned int &Brows, const const vpArray2D &A, const vpArray2D &B, const double &alpha, const vpArray2D &C, const double &beta, vpArray2D &D) { - for (unsigned int r = 0; r < Arows; r++) - for (unsigned int c = 0; c < Bcols; c++) { + for (unsigned int r = 0; r < Arows; ++r) + for (unsigned int c = 0; c < Bcols; ++c) { double sum = 0; - for (unsigned int n = 0; n < Brows; n++) + for (unsigned int n = 0; n < Brows; ++n) sum += A[r][n] * B[n][c] * alpha; D[r][c] = sum + C[c][r] * beta; } @@ -280,10 +278,10 @@ inline void GEMM2<5>(const unsigned int &Arows, const unsigned int &Brows, const const vpArray2D &A, const vpArray2D &B, const double &alpha, const vpArray2D &C, const double &beta, vpArray2D &D) { - for (unsigned int r = 0; r < Arows; r++) - for (unsigned int c = 0; c < Bcols; c++) { + for (unsigned int r = 0; r < Arows; ++r) + for (unsigned int c = 0; c < Bcols; ++c) { double sum = 0; - for (unsigned int n = 0; n < Brows; n++) + for (unsigned int n = 0; n < Brows; ++n) sum += A[n][r] * B[n][c] * alpha; D[r][c] = sum + C[c][r] * beta; } @@ -294,10 +292,10 @@ inline void GEMM2<6>(const unsigned int &Arows, const unsigned int &Brows, const const vpArray2D &A, const vpArray2D &B, const double &alpha, const vpArray2D &C, const double &beta, vpArray2D &D) { - for (unsigned int r = 0; r < Arows; r++) - for (unsigned int c = 0; c < Bcols; c++) { + for (unsigned int r = 0; r < Arows; ++r) + for (unsigned int c = 0; c < Bcols; ++c) { double sum = 0; - for (unsigned int n = 0; n < Brows; n++) + for (unsigned int n = 0; n < Brows; ++n) sum += A[r][n] * B[c][n] * alpha; D[r][c] = sum + C[c][r] * beta; } @@ -308,10 +306,10 @@ inline void GEMM2<7>(const unsigned int &Arows, const unsigned int &Brows, const const vpArray2D &A, const vpArray2D &B, const double &alpha, const vpArray2D &C, const double &beta, vpArray2D &D) { - for (unsigned int r = 0; r < Arows; r++) - for (unsigned int c = 0; c < Bcols; c++) { + for (unsigned int r = 0; r < Arows; ++r) + for (unsigned int c = 0; c < Bcols; ++c) { double sum = 0; - for (unsigned int n = 0; n < Brows; n++) + for (unsigned int n = 0; n < Brows; ++n) sum += A[n][r] * B[c][n] * alpha; D[r][c] = sum + C[c][r] * beta; } @@ -331,7 +329,8 @@ inline void vpTGEMM(const vpArray2D &A, const vpArray2D &B, cons try { if ((Arows != D.getRows()) || (Bcols != D.getCols())) D.resize(Arows, Bcols); - } catch (...) { + } + catch (...) { throw; } @@ -347,7 +346,8 @@ inline void vpTGEMM(const vpArray2D &A, const vpArray2D &B, cons } GEMM2(Arows, Brows, Bcols, A, B, alpha, C, beta, D); - } else { + } + else { GEMM1(Arows, Brows, Bcols, A, B, alpha, D); } } diff --git a/modules/core/include/visp3/core/vpHistogram.h b/modules/core/include/visp3/core/vpHistogram.h index 419b855cc2..2e2b99fdb5 100644 --- a/modules/core/include/visp3/core/vpHistogram.h +++ b/modules/core/include/visp3/core/vpHistogram.h @@ -130,7 +130,7 @@ class VISP_EXPORT vpHistogram h.calculate(I); // Histogram of the gray level image // Print the histogram values - for (int i=0; i < h.getSize(); i ++) + for (int i=0; i < h.getSize(); ++i) printf("%d: %d\n", i, h[i]); \endcode @@ -160,7 +160,7 @@ class VISP_EXPORT vpHistogram h.calculate(I); // Histogram of the gray level image // Print the histogram values - for (int i=0; i < h.getSize(); i ++) + for (int i=0; i < h.getSize(); ++i) printf("%d: %d\n", i, h(i)); \endcode @@ -190,7 +190,7 @@ class VISP_EXPORT vpHistogram h.calculate(I); // Histogram of the gray level image // Print the histogram values - for (int i=0; i < h.getSize(); i ++) + for (int i=0; i < h.getSize(); ++i) printf("%d: %d\n", i, h.get(i)); \endcode @@ -218,7 +218,7 @@ class VISP_EXPORT vpHistogram vpHistogram h; // Set histogram values - for (int i=0; i < h.getSize(); i ++) + for (int i=0; i < h.getSize(); ++i) h.set(i, i*2); // for each level i, set a value of 2*i \endcode @@ -294,7 +294,7 @@ class VISP_EXPORT vpHistogram // Print the histogram values unsigned char *values = h.getValues(); - for (int i=0; i < h.getSize(); i ++) + for (int i=0; i < h.getSize(); ++i) printf("%d: %d\n", i, values[i]); \endcode diff --git a/modules/core/include/visp3/core/vpHomogeneousMatrix.h b/modules/core/include/visp3/core/vpHomogeneousMatrix.h index 00066f19d4..93048abda0 100644 --- a/modules/core/include/visp3/core/vpHomogeneousMatrix.h +++ b/modules/core/include/visp3/core/vpHomogeneousMatrix.h @@ -99,8 +99,8 @@ class vpPoint; M[2][0] = -1; M[2][1] = 0; M[2][2] = 0; M[2][3] = 0.3; std::cout << "M:" << std::endl; - for (unsigned int i = 0; i < M.getRows(); i++) { - for (unsigned int j = 0; j < M.getCols(); j++) { + for (unsigned int i = 0; i < M.getRows(); ++i) { + for (unsigned int j = 0; j < M.getCols(); ++j) { std::cout << M[i][j] << " "; } std::cout << std::endl; diff --git a/modules/core/include/visp3/core/vpImage.h b/modules/core/include/visp3/core/vpImage.h index 85918192d0..606992c43c 100644 --- a/modules/core/include/visp3/core/vpImage.h +++ b/modules/core/include/visp3/core/vpImage.h @@ -361,8 +361,8 @@ template std::ostream &operator<<(std::ostream &s, const vpImage &I std::ios_base::fmtflags original_flags = s.flags(); - for (unsigned int i = 0; i < I.getHeight(); i++) { - for (unsigned int j = 0; j < I.getWidth() - 1; j++) { + for (unsigned int i = 0; i < I.getHeight(); ++i) { + for (unsigned int j = 0; j < I.getWidth() - 1; ++j) { s << std::setw(3) << static_cast(I[i][j]) << " "; } @@ -412,8 +412,8 @@ inline std::ostream &operator<<(std::ostream &s, const vpImage &I) std::ios_base::fmtflags original_flags = s.flags(); - for (unsigned int i = 0; i < I.getHeight(); i++) { - for (unsigned int j = 0; j < I.getWidth() - 1; j++) { + for (unsigned int i = 0; i < I.getHeight(); ++i) { + for (unsigned int j = 0; j < I.getWidth() - 1; ++j) { s << std::setw(4) << static_cast(I[i][j]) << " "; } @@ -439,8 +439,8 @@ inline std::ostream &operator<<(std::ostream &s, const vpImage &I) std::ios_base::fmtflags original_flags = s.flags(); s.precision(9); // http://en.cppreference.com/w/cpp/types/numeric_limits/max_digits10 - for (unsigned int i = 0; i < I.getHeight(); i++) { - for (unsigned int j = 0; j < I.getWidth() - 1; j++) { + for (unsigned int i = 0; i < I.getHeight(); ++i) { + for (unsigned int j = 0; j < I.getWidth() - 1; ++j) { s << I[i][j] << " "; } @@ -466,8 +466,8 @@ inline std::ostream &operator<<(std::ostream &s, const vpImage &I) std::ios_base::fmtflags original_flags = s.flags(); s.precision(17); // http://en.cppreference.com/w/cpp/types/numeric_limits/max_digits10 - for (unsigned int i = 0; i < I.getHeight(); i++) { - for (unsigned int j = 0; j < I.getWidth() - 1; j++) { + for (unsigned int i = 0; i < I.getHeight(); ++i) { + for (unsigned int j = 0; j < I.getWidth() - 1; ++j) { s << I[i][j] << " "; } @@ -620,7 +620,7 @@ template void vpImage::init(unsigned int h, unsigned int w, T { init(h, w); - // for (unsigned int i = 0; i < npixels; i++) + // for (unsigned int i = 0; i < npixels; ++i) // bitmap[i] = value; std::fill(bitmap, bitmap + npixels, value); } @@ -664,7 +664,7 @@ template void vpImage::init(unsigned int h, unsigned int w) throw(vpException(vpException::memoryAllocationError, "cannot allocate row ")); } - for (unsigned int i = 0; i < height; i++) + for (unsigned int i = 0; i < height; ++i) row[i] = bitmap + i * width; } @@ -718,7 +718,7 @@ template void vpImage::init(Type *const array, unsigned int h throw(vpException(vpException::memoryAllocationError, "cannot allocate row ")); } - for (unsigned int i = 0; i < height; i++) { + for (unsigned int i = 0; i < height; ++i) { row[i] = bitmap + i * width; } } @@ -876,7 +876,7 @@ template Type vpImage::getMaxValue(bool onlyFiniteVal) const if (npixels == 0) throw(vpException(vpException::fatalError, "Cannot compute maximum value of an empty image")); Type m = bitmap[0]; - for (unsigned int i = 0; i < npixels; i++) { + for (unsigned int i = 0; i < npixels; ++i) { if (bitmap[i] > m) { m = bitmap[i]; } @@ -899,13 +899,13 @@ template <> inline double vpImage::getMaxValue(bool onlyFiniteVal) const throw(vpException(vpException::fatalError, "Cannot compute maximum value of an empty image")); double m = bitmap[0]; if (onlyFiniteVal) { - for (unsigned int i = 0; i < npixels; i++) { + for (unsigned int i = 0; i < npixels; ++i) { if (bitmap[i] > m && vpMath::isFinite(bitmap[i])) m = bitmap[i]; } } else { - for (unsigned int i = 0; i < npixels; i++) { + for (unsigned int i = 0; i < npixels; ++i) { if (bitmap[i] > m) m = bitmap[i]; } @@ -927,13 +927,13 @@ template <> inline float vpImage::getMaxValue(bool onlyFiniteVal) const throw(vpException(vpException::fatalError, "Cannot compute maximum value of an empty image")); float m = bitmap[0]; if (onlyFiniteVal) { - for (unsigned int i = 0; i < npixels; i++) { + for (unsigned int i = 0; i < npixels; ++i) { if (bitmap[i] > m && vpMath::isFinite(bitmap[i])) m = bitmap[i]; } } else { - for (unsigned int i = 0; i < npixels; i++) { + for (unsigned int i = 0; i < npixels; ++i) { if (bitmap[i] > m) m = bitmap[i]; } @@ -1156,7 +1156,7 @@ template Type vpImage::getMinValue(bool onlyFiniteVal) const if (npixels == 0) throw(vpException(vpException::fatalError, "Cannot compute minimum value of an empty image")); Type m = bitmap[0]; - for (unsigned int i = 0; i < npixels; i++) { + for (unsigned int i = 0; i < npixels; ++i) { if (bitmap[i] < m) { m = bitmap[i]; } @@ -1179,12 +1179,12 @@ template <> inline double vpImage::getMinValue(bool onlyFiniteVal) const throw(vpException(vpException::fatalError, "Cannot compute minimum value of an empty image")); double m = bitmap[0]; if (onlyFiniteVal) { - for (unsigned int i = 0; i < npixels; i++) + for (unsigned int i = 0; i < npixels; ++i) if (bitmap[i] < m && vpMath::isFinite(bitmap[i])) m = bitmap[i]; } else { - for (unsigned int i = 0; i < npixels; i++) + for (unsigned int i = 0; i < npixels; ++i) if (bitmap[i] < m) m = bitmap[i]; } @@ -1205,12 +1205,12 @@ template <> inline float vpImage::getMinValue(bool onlyFiniteVal) const throw(vpException(vpException::fatalError, "Cannot compute minimum value of an empty image")); float m = bitmap[0]; if (onlyFiniteVal) { - for (unsigned int i = 0; i < npixels; i++) + for (unsigned int i = 0; i < npixels; ++i) if (bitmap[i] < m && vpMath::isFinite(bitmap[i])) m = bitmap[i]; } else { - for (unsigned int i = 0; i < npixels; i++) + for (unsigned int i = 0; i < npixels; ++i) if (bitmap[i] < m) m = bitmap[i]; } @@ -1233,7 +1233,7 @@ template void vpImage::getMinMaxValue(Type &min, Type &max, b throw(vpException(vpException::fatalError, "Cannot get minimum/maximum values of an empty image")); min = max = bitmap[0]; - for (unsigned int i = 0; i < npixels; i++) { + for (unsigned int i = 0; i < npixels; ++i) { if (bitmap[i] < min) min = bitmap[i]; if (bitmap[i] > max) @@ -1260,7 +1260,7 @@ template <> inline void vpImage::getMinMaxValue(double &min, double &max min = max = bitmap[0]; if (onlyFiniteVal) { - for (unsigned int i = 0; i < npixels; i++) { + for (unsigned int i = 0; i < npixels; ++i) { if (vpMath::isFinite(bitmap[i])) { if (bitmap[i] < min) min = bitmap[i]; @@ -1270,7 +1270,7 @@ template <> inline void vpImage::getMinMaxValue(double &min, double &max } } else { - for (unsigned int i = 0; i < npixels; i++) { + for (unsigned int i = 0; i < npixels; ++i) { if (bitmap[i] < min) min = bitmap[i]; if (bitmap[i] > max) @@ -1297,7 +1297,7 @@ template <> inline void vpImage::getMinMaxValue(float &min, float &max, b min = max = bitmap[0]; if (onlyFiniteVal) { - for (unsigned int i = 0; i < npixels; i++) { + for (unsigned int i = 0; i < npixels; ++i) { if (vpMath::isFinite(bitmap[i])) { if (bitmap[i] < min) min = bitmap[i]; @@ -1307,7 +1307,7 @@ template <> inline void vpImage::getMinMaxValue(float &min, float &max, b } } else { - for (unsigned int i = 0; i < npixels; i++) { + for (unsigned int i = 0; i < npixels; ++i) { if (bitmap[i] < min) min = bitmap[i]; if (bitmap[i] > max) @@ -1333,7 +1333,7 @@ template <> inline void vpImage::getMinMaxValue(vpRGBf &min, vpRGBf &max min = max = bitmap[0]; if (onlyFiniteVal) { - for (unsigned int i = 0; i < npixels; i++) { + for (unsigned int i = 0; i < npixels; ++i) { if (vpMath::isFinite(bitmap[i].R)) { if (bitmap[i].R < min.R) min.R = bitmap[i].R; @@ -1355,7 +1355,7 @@ template <> inline void vpImage::getMinMaxValue(vpRGBf &min, vpRGBf &max } } else { - for (unsigned int i = 0; i < npixels; i++) { + for (unsigned int i = 0; i < npixels; ++i) { if (bitmap[i].R < min.R) min.R = bitmap[i].R; if (bitmap[i].R > max.R) @@ -1404,8 +1404,8 @@ void vpImage::getMinMaxLoc(vpImagePoint *minLoc, vpImagePoint *maxLoc, Typ Type min = bitmap[0], max = bitmap[0]; vpImagePoint minLoc_, maxLoc_; - for (unsigned int i = 0; i < height; i++) { - for (unsigned int j = 0; j < width; j++) { + for (unsigned int i = 0; i < height; ++i) { + for (unsigned int j = 0; j < width; ++j) { if (row[i][j] < min) { min = row[i][j]; minLoc_.set_ij(i, j); @@ -1451,7 +1451,7 @@ template vpImage &vpImage::operator=(vpImage othe */ template vpImage &vpImage::operator=(const Type &v) { - for (unsigned int i = 0; i < npixels; i++) + for (unsigned int i = 0; i < npixels; ++i) bitmap[i] = v; return *this; @@ -1471,7 +1471,7 @@ template bool vpImage::operator==(const vpImage &I) con // printf("wxh: %dx%d bitmap: %p I.bitmap %p\n", width, height, bitmap, // I.bitmap); - for (unsigned int i = 0; i < npixels; i++) { + for (unsigned int i = 0; i < npixels; ++i) { if (bitmap[i] != I.bitmap[i]) { // std::cout << "differ for pixel " << i << " (" << i%this->height // << ", " << i - i%this->height << ")" << std::endl; @@ -1569,7 +1569,7 @@ template void vpImage::insert(const vpImage &src, const else hsize = src_h - src_ibegin; - for (int i = 0; i < hsize; i++) { + for (int i = 0; i < hsize; ++i) { Type *srcBitmap = src.bitmap + ((src_ibegin + i) * src_w + src_jbegin); Type *destBitmap = this->bitmap + ((dest_ibegin + i) * dest_w + dest_jbegin); @@ -1612,8 +1612,8 @@ template void vpImage::halfSizeImage(vpImage &res) cons unsigned int h = height / 2; unsigned int w = width / 2; res.resize(h, w); - for (unsigned int i = 0; i < h; i++) - for (unsigned int j = 0; j < w; j++) + for (unsigned int i = 0; i < h; ++i) + for (unsigned int j = 0; j < w; ++j) res[i][j] = (*this)[i << 1][j << 1]; } @@ -1644,8 +1644,8 @@ void vpImage::subsample(unsigned int v_scale, unsigned int h_scale, vpImag unsigned int h = height / v_scale; unsigned int w = width / h_scale; sampled.resize(h, w); - for (unsigned int i = 0; i < h; i++) - for (unsigned int j = 0; j < w; j++) + for (unsigned int i = 0; i < h; ++i) + for (unsigned int j = 0; j < w; ++j) sampled[i][j] = (*this)[i * v_scale][j * h_scale]; } @@ -1676,8 +1676,8 @@ template void vpImage::quarterSizeImage(vpImage &res) c unsigned int h = height / 4; unsigned int w = width / 4; res.resize(h, w); - for (unsigned int i = 0; i < h; i++) - for (unsigned int j = 0; j < w; j++) + for (unsigned int i = 0; i < h; ++i) + for (unsigned int j = 0; j < w; ++j) res[i][j] = (*this)[i << 2][j << 2]; } @@ -1720,8 +1720,8 @@ template void vpImage::doubleSizeImage(vpImage &res) res.resize(h, w); - for (int i = 0; i < h; i++) - for (int j = 0; j < w; j++) + for (int i = 0; i < h; ++i) + for (int j = 0; j < w; ++j) res[i][j] = (*this)[i >> 1][j >> 1]; /* @@ -2229,7 +2229,7 @@ template void vpImage::sub(const vpImage &B, vpImagegetWidth() * this->getHeight(); i++) { + for (unsigned int i = 0; i < this->getWidth() * this->getHeight(); ++i) { *(C.bitmap + i) = *(bitmap + i) - *(B.bitmap + i); } } @@ -2261,7 +2261,7 @@ template void vpImage::sub(const vpImage &A, const vpIm throw(vpException(vpException::memoryAllocationError, "vpImage mismatch in vpImage/vpImage subtraction ")); } - for (unsigned int i = 0; i < A.getWidth() * A.getHeight(); i++) { + for (unsigned int i = 0; i < A.getWidth() * A.getHeight(); ++i) { *(C.bitmap + i) = *(A.bitmap + i) - *(B.bitmap + i); } } @@ -2323,7 +2323,7 @@ template <> inline void vpImage::performLut(const unsigned char(& unsigned int step = image_size / nbThreads; unsigned int last_step = image_size - step * (nbThreads - 1); - for (unsigned int index = 0; index < nbThreads; index++) { + for (unsigned int index = 0; index < nbThreads; ++index) { unsigned int start_index = index * step; unsigned int end_index = (index + 1) * step; @@ -2341,17 +2341,17 @@ template <> inline void vpImage::performLut(const unsigned char(& threadpool.push_back(imageLut_thread); } - for (size_t cpt = 0; cpt < threadpool.size(); cpt++) { + for (size_t cpt = 0; cpt < threadpool.size(); ++cpt) { // Wait until thread ends up threadpool[cpt]->join(); } // Delete - for (size_t cpt = 0; cpt < threadpool.size(); cpt++) { + for (size_t cpt = 0; cpt < threadpool.size(); ++cpt) { delete threadpool[cpt]; } - for (size_t cpt = 0; cpt < imageLutParams.size(); cpt++) { + for (size_t cpt = 0; cpt < imageLutParams.size(); ++cpt) { delete imageLutParams[cpt]; } #endif @@ -2410,7 +2410,7 @@ template <> inline void vpImage::performLut(const vpRGBa(&lut)[256], uns unsigned int step = image_size / nbThreads; unsigned int last_step = image_size - step * (nbThreads - 1); - for (unsigned int index = 0; index < nbThreads; index++) { + for (unsigned int index = 0; index < nbThreads; ++index) { unsigned int start_index = index * step; unsigned int end_index = (index + 1) * step; @@ -2428,17 +2428,17 @@ template <> inline void vpImage::performLut(const vpRGBa(&lut)[256], uns threadpool.push_back(imageLut_thread); } - for (size_t cpt = 0; cpt < threadpool.size(); cpt++) { + for (size_t cpt = 0; cpt < threadpool.size(); ++cpt) { // Wait until thread ends up threadpool[cpt]->join(); } // Delete - for (size_t cpt = 0; cpt < threadpool.size(); cpt++) { + for (size_t cpt = 0; cpt < threadpool.size(); ++cpt) { delete threadpool[cpt]; } - for (size_t cpt = 0; cpt < imageLutParams.size(); cpt++) { + for (size_t cpt = 0; cpt < imageLutParams.size(); ++cpt) { delete imageLutParams[cpt]; } #endif diff --git a/modules/core/include/visp3/core/vpImageMorphology.h b/modules/core/include/visp3/core/vpImageMorphology.h index 0325cee9c6..c748209bd9 100644 --- a/modules/core/include/visp3/core/vpImageMorphology.h +++ b/modules/core/include/visp3/core/vpImageMorphology.h @@ -188,9 +188,9 @@ void vpImageMorphology::erosion(vpImage &I, Type value, Type value_out, vp vpImage J(I.getHeight() + 2, I.getWidth() + 2); // Copy I to J and add border - for (unsigned int i = 0; i < J.getHeight(); i++) { + for (unsigned int i = 0; i < J.getHeight(); ++i) { if (i == 0 || i == J.getHeight() - 1) { - for (unsigned int j = 0; j < J.getWidth(); j++) { + for (unsigned int j = 0; j < J.getWidth(); ++j) { J[i][j] = value; } } @@ -202,8 +202,8 @@ void vpImageMorphology::erosion(vpImage &I, Type value, Type value_out, vp } if (connexity == CONNEXITY_4) { - for (unsigned int i = 0; i < I.getHeight(); i++) { - for (unsigned int j = 0; j < I.getWidth(); j++) { + for (unsigned int i = 0; i < I.getHeight(); ++i) { + for (unsigned int j = 0; j < I.getWidth(); ++j) { if (J[i + 1][j + 1] == value) { // Consider 4 neighbors if ((J[i][j + 1] == value_out) || // Top @@ -217,8 +217,8 @@ void vpImageMorphology::erosion(vpImage &I, Type value, Type value_out, vp } } else { - for (unsigned int i = 0; i < I.getHeight(); i++) { - for (unsigned int j = 0; j < I.getWidth(); j++) { + for (unsigned int i = 0; i < I.getHeight(); ++i) { + for (unsigned int j = 0; j < I.getWidth(); ++j) { if (J[i + 1][j + 1] == value) { // Consider 8 neighbors if ((J[i][j] == value_out) || (J[i][j + 1] == value_out) || (J[i][j + 2] == value_out) || @@ -258,9 +258,9 @@ void vpImageMorphology::dilatation(vpImage &I, Type value, Type value_out, vpImage J(I.getHeight() + 2, I.getWidth() + 2); // Copy I to J and add border - for (unsigned int i = 0; i < J.getHeight(); i++) { + for (unsigned int i = 0; i < J.getHeight(); ++i) { if (i == 0 || i == J.getHeight() - 1) { - for (unsigned int j = 0; j < J.getWidth(); j++) { + for (unsigned int j = 0; j < J.getWidth(); ++j) { J[i][j] = value_out; } } @@ -272,8 +272,8 @@ void vpImageMorphology::dilatation(vpImage &I, Type value, Type value_out, } if (connexity == CONNEXITY_4) { - for (unsigned int i = 0; i < I.getHeight(); i++) { - for (unsigned int j = 0; j < I.getWidth(); j++) { + for (unsigned int i = 0; i < I.getHeight(); ++i) { + for (unsigned int j = 0; j < I.getWidth(); ++j) { if (J[i + 1][j + 1] == value_out) { // Consider 4 neighbors if ((J[i][j + 1] == value) || // Top @@ -287,8 +287,8 @@ void vpImageMorphology::dilatation(vpImage &I, Type value, Type value_out, } } else { - for (unsigned int i = 0; i < I.getHeight(); i++) { - for (unsigned int j = 0; j < I.getWidth(); j++) { + for (unsigned int i = 0; i < I.getHeight(); ++i) { + for (unsigned int j = 0; j < I.getWidth(); ++j) { if (J[i + 1][j + 1] == value_out) { // Consider 8 neighbors if ((J[i][j] == value) || (J[i][j + 1] == value) || (J[i][j + 2] == value) || (J[i + 1][j] == value) || @@ -319,10 +319,10 @@ void vpImageMorphology::imageOperation(vpImage &I, const T &null_value, const int offset_x[nbOffset] = { 0, -1, 0, 1, 0 }; int offset_y[nbOffset] = { -1, 0, 0, 0, 1 }; - for (int i = 0; i < height_in; i++) { - for (int j = 0; j < width_in; j++) { + for (int i = 0; i < height_in; ++i) { + for (int j = 0; j < width_in; ++j) { T value = null_value; - for (int k = 0; k < nbOffset; k++) { + for (int k = 0; k < nbOffset; ++k) { value = operation(value, J[i + 1 + offset_y[k]][j + 1 + offset_x[k]]); } @@ -335,10 +335,10 @@ void vpImageMorphology::imageOperation(vpImage &I, const T &null_value, const int offset_x[nbOffset] = { -1, 0, 1,-1, 0, 1,-1, 0, 1 }; int offset_y[nbOffset] = { -1,-1,-1, 0, 0, 0, 1, 1, 1 }; - for (int i = 0; i < height_in; i++) { - for (int j = 0; j < width_in; j++) { + for (int i = 0; i < height_in; ++i) { + for (int j = 0; j < width_in; ++j) { T value = null_value; - for (int k = 0; k < nbOffset; k++) { + for (int k = 0; k < nbOffset; ++k) { value = operation(value, J[i + 1 + offset_y[k]][j + 1 + offset_x[k]]); } @@ -420,7 +420,7 @@ void vpImageMorphology::imageOperation(vpImage &I, const T &(*operation)(cons int halfKernelSize = size / 2; vpImage J = I; - for (int r = 0; r < height_in; r++) { + for (int r = 0; r < height_in; ++r) { // Computing the rows we can explore without going outside the limits of the image int r_iterator_start = -halfKernelSize, r_iterator_stop = halfKernelSize + 1; if (r - halfKernelSize < 0) { @@ -429,7 +429,7 @@ void vpImageMorphology::imageOperation(vpImage &I, const T &(*operation)(cons else if (r + halfKernelSize >= height_in) { r_iterator_stop = height_in - r; } - for (int c = 0; c < width_in; c++) { + for (int c = 0; c < width_in; ++c) { T value = I[r][c]; // Computing the columns we can explore without going outside the limits of the image int c_iterator_start = -halfKernelSize, c_iterator_stop = halfKernelSize + 1; @@ -439,8 +439,8 @@ void vpImageMorphology::imageOperation(vpImage &I, const T &(*operation)(cons else if (c + halfKernelSize >= width_in) { c_iterator_stop = width_in - c; } - 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++) { + 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]); } } diff --git a/modules/core/include/visp3/core/vpImageTools.h b/modules/core/include/visp3/core/vpImageTools.h index 3e20d04e77..99b53db40f 100644 --- a/modules/core/include/visp3/core/vpImageTools.h +++ b/modules/core/include/visp3/core/vpImageTools.h @@ -326,22 +326,22 @@ void vpImageTools::crop(const vpImage &I, double roi_top, double roi_left, crop.resize(r_height, r_width); if (v_scale == 1 && h_scale == 1) { - for (unsigned int i = 0; i < r_height; i++) { + for (unsigned int i = 0; i < r_height; ++i) { void *src = (void *)(I[i + i_min_u] + j_min_u); void *dst = (void *)crop[i]; memcpy(dst, src, r_width * sizeof(Type)); } } else if (h_scale == 1) { - for (unsigned int i = 0; i < r_height; i++) { + for (unsigned int i = 0; i < r_height; ++i) { void *src = (void *)(I[(i + i_min_u) * v_scale] + j_min_u); void *dst = (void *)crop[i]; memcpy(dst, src, r_width * sizeof(Type)); } } else { - for (unsigned int i = 0; i < r_height; i++) { - for (unsigned int j = 0; j < r_width; j++) { + for (unsigned int i = 0; i < r_height; ++i) { + for (unsigned int j = 0; j < r_width; ++j) { crop[i][j] = I[(i + i_min_u) * v_scale][(j + j_min_u) * h_scale]; } } @@ -431,23 +431,23 @@ void vpImageTools::crop(const unsigned char *bitmap, unsigned int width, unsigne crop.resize(r_height, r_width); if (v_scale == 1 && h_scale == 1) { - for (unsigned int i = 0; i < r_height; i++) { + for (unsigned int i = 0; i < r_height; ++i) { void *src = (void *)(bitmap + ((i + i_min_u) * width + j_min_u) * sizeof(Type)); void *dst = (void *)crop[i]; memcpy(dst, src, r_width * sizeof(Type)); } } else if (h_scale == 1) { - for (unsigned int i = 0; i < r_height; i++) { + for (unsigned int i = 0; i < r_height; ++i) { void *src = (void *)(bitmap + ((i + i_min_u) * width * v_scale + j_min_u) * sizeof(Type)); void *dst = (void *)crop[i]; memcpy(dst, src, r_width * sizeof(Type)); } } else { - for (unsigned int i = 0; i < r_height; i++) { + for (unsigned int i = 0; i < r_height; ++i) { unsigned int i_src = (i + i_min_u) * width * v_scale + j_min_u * h_scale; - for (unsigned int j = 0; j < r_width; j++) { + for (unsigned int j = 0; j < r_width; ++j) { void *src = (void *)(bitmap + (i_src + j * h_scale) * sizeof(Type)); void *dst = (void *)&crop[i][j]; memcpy(dst, src, sizeof(Type)); @@ -477,7 +477,7 @@ inline void vpImageTools::binarise(vpImage &I, Type threshold1, Type thres Type v; Type *p = I.bitmap; Type *pend = I.bitmap + I.getWidth() * I.getHeight(); - for (; p < pend; p++) { + for (; p < pend; ++p) { v = *p; if (v < threshold1) *p = value1; @@ -505,7 +505,7 @@ inline void vpImageTools::binarise(vpImage &I, unsigned char thre if (useLUT) { // Construct the LUT unsigned char lut[256]; - for (unsigned int i = 0; i < 256; i++) { + for (unsigned int i = 0; i < 256; ++i) { lut[i] = i < threshold1 ? value1 : (i > threshold2 ? value3 : value2); } @@ -514,7 +514,7 @@ inline void vpImageTools::binarise(vpImage &I, unsigned char thre else { unsigned char *p = I.bitmap; unsigned char *pend = I.bitmap + I.getWidth() * I.getHeight(); - for (; p < pend; p++) { + for (; p < pend; ++p) { unsigned char v = *p; if (v < threshold1) *p = value1; @@ -582,12 +582,12 @@ template void vpUndistortInternalType::vpUndistort_threaded(v Type *dst = undistortSharedData.dst + (height / nthreads * offset) * width; Type *src = undistortSharedData.src; - for (double v = height / nthreads * offset; v < height / nthreads * (offset + 1); v++) { + for (double v = height / nthreads * offset; v < height / nthreads * (offset + 1); ++v) { double deltav = v - v0; // double fr1 = 1.0 + kd * (vpMath::sqr(deltav * invpy)); double fr1 = 1.0 + kud_py2 * deltav * deltav; - for (double u = 0; u < width; u++) { + for (double u = 0; u < width; ++u) { // computation of u,v : corresponding pixel coordinates in I. double deltau = u - u0; // double fr2 = fr1 + kd * (vpMath::sqr(deltau * invpx)); @@ -677,7 +677,7 @@ void vpImageTools::undistort(const vpImage &I, const vpCameraParameters &c vpUndistortInternalType *undistortSharedData = new vpUndistortInternalType[nthreads]; - for (unsigned int i = 0; i < nthreads; i++) { + for (unsigned int i = 0; i < nthreads; ++i) { // Each thread works on a different set of data. // vpTRACE("create thread %d", i); undistortSharedData[i].src = I.bitmap; @@ -692,12 +692,12 @@ void vpImageTools::undistort(const vpImage &I, const vpCameraParameters &c } /* Wait on the other threads */ - for (unsigned int i = 0; i < nthreads; i++) { + for (unsigned int i = 0; i < nthreads; ++i) { // vpTRACE("join thread %d", i); threadpool[i]->join(); } - for (unsigned int i = 0; i < nthreads; i++) { + for (unsigned int i = 0; i < nthreads; ++i) { delete threadpool[i]; } @@ -732,12 +732,12 @@ void vpImageTools::undistort(const vpImage &I, const vpCameraParameters &c double kud_py2 = kud * invpy * invpy; Type *dst = undistI.bitmap; - for (double v = 0; v < height; v++) { + for (double v = 0; v < height; ++v) { double deltav = v - v0; // double fr1 = 1.0 + kd * (vpMath::sqr(deltav * invpy)); double fr1 = 1.0 + kud_py2 * deltav * deltav; - for (double u = 0; u < width; u++) { + for (double u = 0; u < width; ++u) { // computation of u,v : corresponding pixel coordinates in I. double deltau = u - u0; // double fr2 = fr1 + kd * (vpMath::sqr(deltau * invpx)); @@ -797,12 +797,12 @@ void vpImageTools::undistort(const vpImage &I, const vpCameraParameters &c return; } - for (int v = 0; v < height; v++) { - for (int u = 0; u < height; u++) { - double r2 = vpMath::sqr(((double)u - u0) / px) + - vpMath::sqr(((double)v - v0) / py); - double u_double = ((double)u - u0) * (1.0 + kd * r2) + u0; - double v_double = ((double)v - v0) * (1.0 + kd * r2) + v0; + for (int v = 0; v < height; ++v) { + for (int u = 0; u < height; ++u) { + double r2 = vpMath::sqr((static_cast(u) - u0) / px) + + vpMath::sqr((static_cast(v) - v0) / py); + double u_double = (static_cast(u) - u0) * (1.0 + kd * r2) + u0; + double v_double = (static_cast(v) - v0) * (1.0 + kd * r2) + v0; undistI[v][u] = I.getPixelBI((float)u_double, (float)v_double); } } @@ -841,7 +841,7 @@ template void vpImageTools::flip(const vpImage &I, vpImage void vpImageTools::flip(vpImage &I) vpImage Ibuf; Ibuf.resize(1, width); - for (unsigned int i = 0; i < height / 2; i++) { + for (unsigned int i = 0; i < height / 2; ++i) { memcpy(Ibuf.bitmap, I.bitmap + i * width, width * sizeof(Type)); memcpy(I.bitmap + i * width, I.bitmap + (height - 1 - i) * width, width * sizeof(Type)); @@ -967,7 +967,7 @@ inline void vpImageTools::resizeBicubic(const vpImage &I, vpImage(reinterpret_cast(&p00)[c]), static_cast(reinterpret_cast(&p01)[c]), static_cast(reinterpret_cast(&p02)[c]), @@ -1029,7 +1029,7 @@ inline void vpImageTools::resizeBilinear(const vpImage &I, vpImage(reinterpret_cast(&I[v0][u0])[c]), static_cast(reinterpret_cast(&I[v1][u1])[c]), xFrac); float col1 = lerp(static_cast(reinterpret_cast(&I[v2][u2])[c]), @@ -1119,12 +1119,12 @@ void vpImageTools::resize(const vpImage &I, vpImage &Ires, const vpI } #pragma omp parallel for schedule(dynamic) #endif - for (int i = 0; i < static_cast(Ires.getHeight()); i++) { + for (int i = 0; i < static_cast(Ires.getHeight()); ++i) { const float v = (i + half) * scaleY - half; const float v0 = std::floor(v); const float yFrac = v - v0; - for (unsigned int j = 0; j < Ires.getWidth(); j++) { + for (unsigned int j = 0; j < Ires.getWidth(); ++j) { const float u = (j + half) * scaleX - half; const float u0 = std::floor(u); const float xFrac = u - u0; @@ -1174,11 +1174,11 @@ inline void vpImageTools::resize(const vpImage &I, vpImage(Ires.getHeight()); i++) { + for (int i = 0; i < static_cast(Ires.getHeight()); ++i) { float v = (i + half) * scaleY - half; float yFrac = v - static_cast(v); - for (unsigned int j = 0; j < Ires.getWidth(); j++) { + for (unsigned int j = 0; j < Ires.getWidth(); ++j) { float u = (j + half) * scaleX - half; float xFrac = u - static_cast(u); @@ -1224,11 +1224,11 @@ inline void vpImageTools::resize(const vpImage &I, vpImage &Ires } #pragma omp parallel for schedule(dynamic) #endif - for (int i = 0; i < static_cast(Ires.getHeight()); i++) { + for (int i = 0; i < static_cast(Ires.getHeight()); ++i) { float v = (i + half) * scaleY - half; float yFrac = v - static_cast(v); - for (unsigned int j = 0; j < Ires.getWidth(); j++) { + for (unsigned int j = 0; j < Ires.getWidth(); ++j) { float u = (j + half) * scaleX - half; float xFrac = u - static_cast(u); @@ -1335,11 +1335,11 @@ void vpImageTools::warpNN(const vpImage &src, const vpMatrix &T, vpImage((src.getWidth() - 1) * precision) + 0x8000; if (affine) { - for (unsigned int i = 0; i < dst.getHeight(); i++) { + for (unsigned int i = 0; i < dst.getHeight(); ++i) { int32_t xi = a2_i32; int32_t yi = a5_i32; - for (unsigned int j = 0; j < dst.getWidth(); j++) { + for (unsigned int j = 0; j < dst.getWidth(); ++j) { if (yi >= 0 && yi < height_1_i32 && xi >= 0 && xi < width_1_i32) { float x_ = (xi >> nbits) + (xi & 0xFFFF) * precision_1; float y_ = (yi >> nbits) + (yi & 0xFFFF) * precision_1; @@ -1358,12 +1358,12 @@ void vpImageTools::warpNN(const vpImage &src, const vpMatrix &T, vpImage= 0 && yi <= (static_cast(src.getHeight()) - 1) * wi && xi >= 0 && xi <= (static_cast(src.getWidth()) - 1) * wi) { float w_ = (wi >> nbits) + (wi & 0xFFFF) * precision_1; @@ -1398,8 +1398,8 @@ void vpImageTools::warpNN(const vpImage &src, const vpMatrix &T, vpImage &src, const vpMatrix &T, vpIma int64_t width_i64 = static_cast(src.getWidth() * precision); if (affine) { - for (unsigned int i = 0; i < dst.getHeight(); i++) { + for (unsigned int i = 0; i < dst.getHeight(); ++i) { int64_t xi_ = a2_i64; int64_t yi_ = a5_i64; - for (unsigned int j = 0; j < dst.getWidth(); j++) { + for (unsigned int j = 0; j < dst.getWidth(); ++j) { if (yi_ >= 0 && yi_ < height_i64 && xi_ >= 0 && xi_ < width_i64) { const int64_t xi_lower = xi_ & (~0xFFFF); const int64_t yi_lower = yi_ & (~0xFFFF); @@ -1499,12 +1499,12 @@ void vpImageTools::warpLinear(const vpImage &src, const vpMatrix &T, vpIma } } else { - for (unsigned int i = 0; i < dst.getHeight(); i++) { + for (unsigned int i = 0; i < dst.getHeight(); ++i) { int64_t xi = a2_i64; int64_t yi = a5_i64; int64_t wi = a8_i64; - for (unsigned int j = 0; j < dst.getWidth(); j++) { + for (unsigned int j = 0; j < dst.getWidth(); ++j) { if (wi != 0 && yi >= 0 && yi <= (static_cast(src.getHeight()) - 1) * wi && xi >= 0 && xi <= (static_cast(src.getWidth()) - 1) * wi) { const float wi_ = (wi >> nbits) + (wi & 0xFFFF) * precision_1; @@ -1566,8 +1566,8 @@ void vpImageTools::warpLinear(const vpImage &src, const vpMatrix &T, vpIma double a7 = affine ? 0.0 : T[2][1]; double a8 = affine ? 1.0 : T[2][2]; - for (unsigned int i = 0; i < dst.getHeight(); i++) { - for (unsigned int j = 0; j < dst.getWidth(); j++) { + for (unsigned int i = 0; i < dst.getHeight(); ++i) { + for (unsigned int j = 0; j < dst.getWidth(); ++j) { double x = a0 * (centerCorner ? j + 0.5 : j) + a1 * (centerCorner ? i + 0.5 : i) + a2; double y = a3 * (centerCorner ? j + 0.5 : j) + a4 * (centerCorner ? i + 0.5 : i) + a5; double w = a6 * (centerCorner ? j + 0.5 : j) + a7 * (centerCorner ? i + 0.5 : i) + a8; @@ -1644,11 +1644,11 @@ inline void vpImageTools::warpLinear(const vpImage &src, const vpMatrix int64_t width_i64 = static_cast(src.getWidth() * precision); if (affine) { - for (unsigned int i = 0; i < dst.getHeight(); i++) { + for (unsigned int i = 0; i < dst.getHeight(); ++i) { int64_t xi = a2_i64; int64_t yi = a5_i64; - for (unsigned int j = 0; j < dst.getWidth(); j++) { + for (unsigned int j = 0; j < dst.getWidth(); ++j) { if (yi >= 0 && yi < height_i64 && xi >= 0 && xi < width_i64) { const int64_t xi_lower = xi & (~0xFFFF); const int64_t yi_lower = yi & (~0xFFFF); @@ -1725,12 +1725,12 @@ inline void vpImageTools::warpLinear(const vpImage &src, const vpMatrix } } else { - for (unsigned int i = 0; i < dst.getHeight(); i++) { + for (unsigned int i = 0; i < dst.getHeight(); ++i) { int64_t xi = a2_i64; int64_t yi = a5_i64; int64_t wi = a8_i64; - for (unsigned int j = 0; j < dst.getWidth(); j++) { + for (unsigned int j = 0; j < dst.getWidth(); ++j) { if (yi >= 0 && yi <= (static_cast(src.getHeight()) - 1) * wi && xi >= 0 && xi <= (static_cast(src.getWidth()) - 1) * wi) { const float wi_ = (wi >> nbits) + (wi & 0xFFFF) * precision_1; @@ -1810,8 +1810,8 @@ inline void vpImageTools::warpLinear(const vpImage &src, const vpMatrix double a7 = affine ? 0.0 : T[2][1]; double a8 = affine ? 1.0 : T[2][2]; - for (unsigned int i = 0; i < dst.getHeight(); i++) { - for (unsigned int j = 0; j < dst.getWidth(); j++) { + for (unsigned int i = 0; i < dst.getHeight(); ++i) { + for (unsigned int j = 0; j < dst.getWidth(); ++j) { double x = a0 * (centerCorner ? j + 0.5 : j) + a1 * (centerCorner ? i + 0.5 : i) + a2; double y = a3 * (centerCorner ? j + 0.5 : j) + a4 * (centerCorner ? i + 0.5 : i) + a5; double w = a6 * (centerCorner ? j + 0.5 : j) + a7 * (centerCorner ? i + 0.5 : i) + a8; diff --git a/modules/core/include/visp3/core/vpIoTools.h b/modules/core/include/visp3/core/vpIoTools.h index 2f37741dfc..b204c83c6e 100644 --- a/modules/core/include/visp3/core/vpIoTools.h +++ b/modules/core/include/visp3/core/vpIoTools.h @@ -85,7 +85,7 @@ struct NpyArray shape(_shape), word_size(_word_size), fortran_order(_fortran_order) { num_vals = 1; - for (size_t i = 0; i < shape.size(); i++) num_vals *= shape[i]; + for (size_t i = 0; i < shape.size(); ++i) num_vals *= shape[i]; data_holder = std::shared_ptr >( new std::vector(num_vals * word_size)); } @@ -139,7 +139,7 @@ VISP_EXPORT NpyArray npy_load(std::string fname); template std::vector &operator+=(std::vector &lhs, const T rhs) { //write in little endian - for (size_t byte = 0; byte < sizeof(T); byte++) { + for (size_t byte = 0; byte < sizeof(T); ++byte) { char val = *((char *)&rhs+byte); lhs.push_back(val); } @@ -157,7 +157,7 @@ template<> inline std::vector &operator+=(std::vector &lhs, const ch //write in little endian size_t len = strlen(rhs); lhs.reserve(len); - for (size_t byte = 0; byte < len; byte++) { + for (size_t byte = 0; byte < len; ++byte) { lhs.push_back(rhs[byte]); } return lhs; @@ -196,7 +196,7 @@ template void npy_save(std::string fname, const T *data, const std:: assert(true_data_shape.size() != shape.size()); } - for (size_t i = 1; i < shape.size(); i++) { + for (size_t i = 1; i < shape.size(); ++i) { if (shape[i] != true_data_shape[i]) { std::cout<<"libnpy error: npy_save attempting to append misshaped data to "< std::vector create_npy_header(const std::vector::ransac(unsigned int npts, const vpColVector &x, while (degenerate == true) { // Generate s random indicies in the range 1..npts - for (unsigned int i = 0; i < s; i++) - ind[i] = (unsigned int)ceil(random() * npts) - 1; + for (unsigned int i = 0; i < s; ++i) { + ind[i] = static_cast(ceil(random() * npts)) - 1; + } // Test that these points are not a degenerate configuration. degenerate = vpTransformation::degenerateConfiguration(x, ind); @@ -173,7 +174,7 @@ bool vpRansac::ransac(unsigned int npts, const vpColVector &x, *residual = 0.0; } ninliers = 0; - for (unsigned int i = 0; i < npts; i++) { + for (unsigned int i = 0; i < npts; ++i) { double resid = fabs(d[i]); if (resid < t) { inliers[i] = 1; diff --git a/modules/core/include/visp3/core/vpRotationMatrix.h b/modules/core/include/visp3/core/vpRotationMatrix.h index 457ffd9ac6..d31f05ad21 100644 --- a/modules/core/include/visp3/core/vpRotationMatrix.h +++ b/modules/core/include/visp3/core/vpRotationMatrix.h @@ -74,8 +74,8 @@ R[2][0] = -1; R[2][1] = 0; R[2][2] = 0; std::cout << "R:" << std::endl; - for (unsigned int i = 0; i < R.getRows(); i++) { - for (unsigned int j = 0; j < R.getCols(); j++) { + for (unsigned int i = 0; i < R.getRows(); ++i) { + for (unsigned int j = 0; j < R.getCols(); ++j) { std::cout << R[i][j] << " "; } std::cout << std::endl; diff --git a/modules/core/include/visp3/core/vpRotationVector.h b/modules/core/include/visp3/core/vpRotationVector.h index a2a34b7506..49c3daae09 100644 --- a/modules/core/include/visp3/core/vpRotationVector.h +++ b/modules/core/include/visp3/core/vpRotationVector.h @@ -127,7 +127,7 @@ class VISP_EXPORT vpRotationVector : public vpArray2D vpRotationVector &operator=(const vpRotationVector &v) { resize(v.size(), 1); - for (unsigned int i = 0; i < v.size(); i++) { + for (unsigned int i = 0; i < v.size(); ++i) { data[i] = v.data[i]; } return *this; diff --git a/modules/core/include/visp3/core/vpRowVector.h b/modules/core/include/visp3/core/vpRowVector.h index 396b89d18f..873eac222a 100644 --- a/modules/core/include/visp3/core/vpRowVector.h +++ b/modules/core/include/visp3/core/vpRowVector.h @@ -73,7 +73,7 @@ int main() v[0] = -1; v[1] = -2.1; v[2] = -3; std::cout << "v:" << std::endl; - for (unsigned int i = 0; i < v.size(); i++) { + for (unsigned int i = 0; i < v.size(); ++i) { std::cout << v[i] << " "; } std::cout << std::endl; @@ -169,7 +169,7 @@ class VISP_EXPORT vpRowVector : public vpArray2D \code vpRowVector r1; - for (unsigned int i=0; i<4; i++) + for (unsigned int i=0; i<4; ++i) r1.stack(i); // r1 is equal to [0 1 2 3] vpRowVector r2 = r1.extract(1, 3); diff --git a/modules/core/src/camera/vpCameraParameters.cpp b/modules/core/src/camera/vpCameraParameters.cpp index 9abf1488f2..c87fa34109 100644 --- a/modules/core/src/camera/vpCameraParameters.cpp +++ b/modules/core/src/camera/vpCameraParameters.cpp @@ -345,7 +345,7 @@ void vpCameraParameters::init(const vpCameraParameters &c) { *this = c; } */ void vpCameraParameters::initFromCalibrationMatrix(const vpMatrix &K) { - if (K.getRows() != 3 || K.getCols() != 3) { + if ((K.getRows() != 3) || (K.getCols() != 3)) { throw vpException(vpException::dimensionError, "bad size for calibration matrix"); } if (std::fabs(K[2][2] - 1.0) > std::numeric_limits::epsilon()) { @@ -393,8 +393,8 @@ void vpCameraParameters::initFromFov(const unsigned int &w, const unsigned int & const double &vfov) { m_projModel = vpCameraParameters::perspectiveProjWithoutDistortion; - m_u0 = (double)w / 2.; - m_v0 = (double)h / 2.; + m_u0 = static_cast(w) / 2.; + m_v0 = static_cast(h) / 2.; m_px = m_u0 / tan(hfov / 2); m_py = m_v0 / tan(vfov / 2); m_kud = 0; @@ -436,39 +436,55 @@ vpCameraParameters &vpCameraParameters::operator=(const vpCameraParameters &cam) */ bool vpCameraParameters::operator==(const vpCameraParameters &c) const { - if (m_projModel != c.m_projModel) + if (m_projModel != c.m_projModel) { return false; + } - if (!vpMath::equal(m_px, c.m_px, std::numeric_limits::epsilon()) || - !vpMath::equal(m_py, c.m_py, std::numeric_limits::epsilon()) || - !vpMath::equal(m_u0, c.m_u0, std::numeric_limits::epsilon()) || - !vpMath::equal(m_v0, c.m_v0, std::numeric_limits::epsilon()) || - !vpMath::equal(m_kud, c.m_kud, std::numeric_limits::epsilon()) || - !vpMath::equal(m_kdu, c.m_kdu, std::numeric_limits::epsilon()) || - !vpMath::equal(m_inv_px, c.m_inv_px, std::numeric_limits::epsilon()) || - !vpMath::equal(m_inv_py, c.m_inv_py, std::numeric_limits::epsilon())) + // maximum allowed conditional operators shall be maximum 3 + if ((!vpMath::equal(m_px, c.m_px, std::numeric_limits::epsilon())) || + (!vpMath::equal(m_py, c.m_py, std::numeric_limits::epsilon())) || + (!vpMath::equal(m_u0, c.m_u0, std::numeric_limits::epsilon()))) { + return false; + } + if ((!vpMath::equal(m_v0, c.m_v0, std::numeric_limits::epsilon())) || + (!vpMath::equal(m_kud, c.m_kud, std::numeric_limits::epsilon())) || + (!vpMath::equal(m_kdu, c.m_kdu, std::numeric_limits::epsilon()))) { + return false; + } + if ((!vpMath::equal(m_inv_px, c.m_inv_px, std::numeric_limits::epsilon())) || + (!vpMath::equal(m_inv_py, c.m_inv_py, std::numeric_limits::epsilon()))) { return false; + } - if (m_dist_coefs.size() != c.m_dist_coefs.size()) + if (m_dist_coefs.size() != c.m_dist_coefs.size()) { return false; + } - for (unsigned int i = 0; i < m_dist_coefs.size(); i++) - if (!vpMath::equal(m_dist_coefs[i], c.m_dist_coefs[i], std::numeric_limits::epsilon())) + unsigned int m_dist_coefs_size = m_dist_coefs.size(); + for (unsigned int i = 0; i < m_dist_coefs_size; ++i) { + if (!vpMath::equal(m_dist_coefs[i], c.m_dist_coefs[i], std::numeric_limits::epsilon())) { return false; + } + } - if (m_isFov != c.m_isFov || !vpMath::equal(m_hFovAngle, c.m_hFovAngle, std::numeric_limits::epsilon()) || - !vpMath::equal(m_vFovAngle, c.m_vFovAngle, std::numeric_limits::epsilon()) || m_width != c.m_width || - m_height != c.m_height) + if ( (m_isFov != c.m_isFov) || (!vpMath::equal(m_hFovAngle, c.m_hFovAngle, std::numeric_limits::epsilon())) || + (!vpMath::equal(m_vFovAngle, c.m_vFovAngle, std::numeric_limits::epsilon()))) { + return false; + } + if ((m_width != c.m_width) || (m_height != c.m_height)) { return false; + } - if (m_fovNormals.size() != c.m_fovNormals.size()) + if (m_fovNormals.size() != c.m_fovNormals.size()) { return false; + } std::vector::const_iterator it1 = m_fovNormals.begin(); std::vector::const_iterator it2 = c.m_fovNormals.begin(); - for (; it1 != m_fovNormals.end() && it2 != c.m_fovNormals.end(); ++it1, ++it2) { - if (*it1 != *it2) + for (; (it1 != m_fovNormals.end()) && (it2 != c.m_fovNormals.end()); ++it1, ++it2) { + if (*it1 != *it2) { return false; + } } return true; @@ -487,15 +503,16 @@ bool vpCameraParameters::operator!=(const vpCameraParameters &c) const { return */ void vpCameraParameters::computeFov(const unsigned int &w, const unsigned int &h) { - if ((!m_isFov || w != m_width || h != m_height) && w != 0 && h != 0) { + bool cond1 = (!m_isFov) || (w != m_width) || (h != m_height); + if ( cond1 && (w != 0) && (h != 0)) { m_fovNormals = std::vector(4); m_isFov = true; - double hFovAngle = atan(((double)w - m_u0) * (1.0 / m_px)); - double vFovAngle = atan((m_v0) * (1.0 / m_py)); - double minushFovAngle = atan((m_u0) * (1.0 / m_px)); - double minusvFovAngle = atan(((double)h - m_v0) * (1.0 / m_py)); + double hFovAngle = atan((static_cast(w) - m_u0) * (1.0 / m_px)); + double vFovAngle = atan(m_v0 * (1.0 / m_py)); + double minushFovAngle = atan(m_u0 * (1.0 / m_px)); + double minusvFovAngle = atan((static_cast(h) - m_v0) * (1.0 / m_py)); m_width = w; m_height = h; @@ -586,6 +603,7 @@ vpMatrix vpCameraParameters::get_K_inverse() const */ void vpCameraParameters::printParameters() { + unsigned int m_dist_coefs_size = m_dist_coefs.size(); std::ios::fmtflags original_flags(std::cout.flags()); switch (m_projModel) { case vpCameraParameters::perspectiveProjWithoutDistortion: @@ -604,8 +622,10 @@ void vpCameraParameters::printParameters() break; case vpCameraParameters::ProjWithKannalaBrandtDistortion: std::cout << " Coefficients: "; - for (unsigned int i = 0; i < m_dist_coefs.size(); i++) + // --comment: unsigned int m_dist_coefs_size = m_dist_coefs.size() + for (unsigned int i = 0; i < m_dist_coefs_size; ++i) { std::cout << " " << m_dist_coefs[i]; + } std::cout << std::endl; break; } @@ -636,17 +656,23 @@ VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpCameraParameters os << " kud = " << cam.get_kud() << std::endl; os << " kdu = " << cam.get_kdu() << std::endl; os.flags(original_flags); // restore os to standard state - } break; + } + break; case vpCameraParameters::ProjWithKannalaBrandtDistortion: { os << "Camera parameters for projection with Kannala-Brandt distortion:" << std::endl; os << " px = " << cam.get_px() << "\t py = " << cam.get_py() << std::endl; os << " u0 = " << cam.get_u0() << "\t v0 = " << cam.get_v0() << std::endl; os << " Coefficients: "; std::vector tmp_coefs = cam.getKannalaBrandtDistortionCoefficients(); - for (unsigned int i = 0; i < tmp_coefs.size(); i++) + unsigned int tmp_coefs_size = tmp_coefs.size(); + for (unsigned int i = 0; i < tmp_coefs_size; ++i) { os << " " << tmp_coefs[i]; + } os << std::endl; - } break; + } + break; + default: + std::cout << "Unidentified camera parameters model" << std::endl; } return os; } diff --git a/modules/core/src/camera/vpColorDepthConversion.cpp b/modules/core/src/camera/vpColorDepthConversion.cpp index 43fbc333e9..797c1b2639 100644 --- a/modules/core/src/camera/vpColorDepthConversion.cpp +++ b/modules/core/src/camera/vpColorDepthConversion.cpp @@ -196,9 +196,9 @@ vpImagePoint vpColorDepthConversion::projectColorToDepth( // search along line for the depth pixel that it's projected pixel is the closest to the input pixel auto min_dist = -1.; - for (auto curr_pixel = start_pixel; curr_pixel.inSegment(start_pixel, end_pixel) && curr_pixel != end_pixel; + for (auto curr_pixel = start_pixel; curr_pixel.inSegment(start_pixel, end_pixel) && (curr_pixel != end_pixel); curr_pixel = curr_pixel.nextInSegment(start_pixel, end_pixel)) { - const auto depth = depth_scale * data[static_cast(curr_pixel.get_v() * depth_width + curr_pixel.get_u())]; + const auto depth = depth_scale * data[static_cast((curr_pixel.get_v() * depth_width) + curr_pixel.get_u())]; if (std::fabs(depth) <= std::numeric_limits::epsilon()) continue; @@ -208,7 +208,7 @@ vpImagePoint vpColorDepthConversion::projectColorToDepth( const auto new_dist = vpMath::sqr(projected_pixel.get_v() - from_pixel.get_v()) + vpMath::sqr(projected_pixel.get_u() - from_pixel.get_u()); - if (new_dist < min_dist || min_dist < 0) { + if ( (new_dist < min_dist) || (min_dist < 0) ) { min_dist = new_dist; depth_pixel = curr_pixel; } diff --git a/modules/core/src/camera/vpMeterPixelConversion.cpp b/modules/core/src/camera/vpMeterPixelConversion.cpp index 62282e380a..05a3b8386d 100644 --- a/modules/core/src/camera/vpMeterPixelConversion.cpp +++ b/modules/core/src/camera/vpMeterPixelConversion.cpp @@ -66,7 +66,7 @@ void vpMeterPixelConversion::convertLine(const vpCameraParameters &cam, const do } theta_p = atan2(cam.m_px * si, cam.m_py * co); - rho_p = (cam.m_px * cam.m_py * rho_m + cam.m_u0 * cam.m_py * co + cam.m_v0 * cam.m_px * si); + rho_p = ((cam.m_px * cam.m_py * rho_m) + (cam.m_u0 * cam.m_py * co) + (cam.m_v0 * cam.m_px * si)); rho_p /= d; } diff --git a/modules/core/src/camera/vpPixelMeterConversion.cpp b/modules/core/src/camera/vpPixelMeterConversion.cpp index 670e1cf131..a116ca6651 100644 --- a/modules/core/src/camera/vpPixelMeterConversion.cpp +++ b/modules/core/src/camera/vpPixelMeterConversion.cpp @@ -87,7 +87,7 @@ void vpPixelMeterConversion::convertLine(const vpCameraParameters &cam, const do throw(vpException(vpException::divideByZeroError, "division by zero")); } theta_m = atan2(si * cam.m_py, co * cam.m_px); - rho_m = (rho_p - cam.m_u0 * co - cam.m_v0 * si) / sqrt(d); + rho_m = (rho_p - (cam.m_u0 * co) - (cam.m_v0 * si)) / sqrt(d); } /*! @@ -134,13 +134,13 @@ void vpPixelMeterConversion::convertMoment(const vpCameraParameters &cam, unsign double yc = -cam.m_v0; double xc = -cam.m_u0; - for (unsigned int k = 0; k < order; k++) { // iteration correspondant e l'ordre du moment - for (unsigned int p = 0; p < order; p++) { // iteration en X - for (unsigned int q = 0; q < order; q++) { // iteration en Y - if (p + q == k) { // on est bien dans la matrice triangulaire superieure + for (unsigned int k = 0; k < order; ++k) { // iteration correspondant e l'ordre du moment + for (unsigned int p = 0; p < order; ++p) { // iteration en X + for (unsigned int q = 0; q < order; ++q) { // iteration en Y + if ((p + q) == k) { // on est bien dans la matrice triangulaire superieure m[p][q] = 0; // initialisation e 0 - for (unsigned int r = 0; r <= p; r++) { // somme externe - for (unsigned int t = 0; t <= q; t++) { // somme interne + for (unsigned int r = 0; r <= p; ++r) { // somme externe + for (unsigned int t = 0; t <= q; ++t) { // somme interne m[p][q] += static_cast(vpMath::comb(p, r)) * static_cast(vpMath::comb(q, t)) * pow(xc, static_cast(p - r)) * pow(yc, static_cast(q - t)) * moment_pixel[r][t]; } @@ -150,20 +150,20 @@ void vpPixelMeterConversion::convertMoment(const vpCameraParameters &cam, unsign } } - for (unsigned int k = 0; k < order; k++) { // iteration correspondant e l'ordre du moment - for (unsigned int p = 0; p < order; p++) { - for (unsigned int q = 0; q < order; q++) { - if (p + q == k) { + for (unsigned int k = 0; k < order; ++k) { // iteration correspondant e l'ordre du moment + for (unsigned int p = 0; p < order; ++p) { + for (unsigned int q = 0; q < order; ++q) { + if ((p + q) == k) { m[p][q] *= pow(cam.m_inv_px, static_cast(1 + p)) * pow(cam.m_inv_py, static_cast(1 + q)); } } } } - for (unsigned int k = 0; k < order; k++) { // iteration correspondant e l'ordre du moment - for (unsigned int p = 0; p < order; p++) { - for (unsigned int q = 0; q < order; q++) { - if (p + q == k) { + for (unsigned int k = 0; k < order; ++k) { // iteration correspondant e l'ordre du moment + for (unsigned int p = 0; p < order; ++p) { + for (unsigned int q = 0; q < order; ++q) { + if ((p + q) == k) { moment_meter[p][q] = m[p][q]; } } @@ -249,13 +249,13 @@ void vpPixelMeterConversion::convertMoment(const cv::Mat &cameraMatrix, unsigned double yc = -v0; double xc = -u0; - for (unsigned int k = 0; k < order; k++) { // iteration correspondant e l'ordre du moment - for (unsigned int p = 0; p < order; p++) { // iteration en X - for (unsigned int q = 0; q < order; q++) { // iteration en Y + for (unsigned int k = 0; k < order; ++k) { // iteration correspondant e l'ordre du moment + for (unsigned int p = 0; p < order; ++p) { // iteration en X + for (unsigned int q = 0; q < order; ++q) { // iteration en Y if (p + q == k) { // on est bien dans la matrice triangulaire superieure m[p][q] = 0; // initialisation e 0 - for (unsigned int r = 0; r <= p; r++) { // somme externe - for (unsigned int t = 0; t <= q; t++) { // somme interne + for (unsigned int r = 0; r <= p; ++r) { // somme externe + for (unsigned int t = 0; t <= q; ++t) { // somme interne m[p][q] += static_cast(vpMath::comb(p, r)) * static_cast(vpMath::comb(q, t)) * pow(xc, static_cast(p - r)) * pow(yc, static_cast(q - t)) * moment_pixel[r][t]; } @@ -265,9 +265,9 @@ void vpPixelMeterConversion::convertMoment(const cv::Mat &cameraMatrix, unsigned } } - for (unsigned int k = 0; k < order; k++) { // iteration correspondant e l'ordre du moment - for (unsigned int p = 0; p < order; p++) { - for (unsigned int q = 0; q < order; q++) { + for (unsigned int k = 0; k < order; ++k) { // iteration correspondant e l'ordre du moment + for (unsigned int p = 0; p < order; ++p) { + for (unsigned int q = 0; q < order; ++q) { if (p + q == k) { m[p][q] *= pow(inv_px, static_cast(1 + p)) * pow(inv_py, static_cast(1 + q)); } @@ -275,9 +275,9 @@ void vpPixelMeterConversion::convertMoment(const cv::Mat &cameraMatrix, unsigned } } - for (unsigned int k = 0; k < order; k++) { // iteration correspondant e l'ordre du moment - for (unsigned int p = 0; p < order; p++) { - for (unsigned int q = 0; q < order; q++) { + for (unsigned int k = 0; k < order; ++k) { // iteration correspondant e l'ordre du moment + for (unsigned int p = 0; p < order; ++p) { + for (unsigned int q = 0; q < order; ++q) { if (p + q == k) { moment_meter[p][q] = m[p][q]; } diff --git a/modules/core/src/camera/vpXmlParserCamera.cpp b/modules/core/src/camera/vpXmlParserCamera.cpp index 3c30c60978..42200c9530 100644 --- a/modules/core/src/camera/vpXmlParserCamera.cpp +++ b/modules/core/src/camera/vpXmlParserCamera.cpp @@ -507,7 +507,7 @@ class vpXmlParserCamera::Impl int check = validation / 32; int j = 0; - for (int i = 0; i < 5; i++) { + for (int i = 0; i < 5; ++i) { int bit = check % 2; // if bit == 1 => the corresponding distortion coefficient is present. if (!bit) fixed_distortion_coeffs.push_back(0.); diff --git a/modules/core/src/display/vpDisplay.cpp b/modules/core/src/display/vpDisplay.cpp index ecabea2c95..5a02096ac5 100644 --- a/modules/core/src/display/vpDisplay.cpp +++ b/modules/core/src/display/vpDisplay.cpp @@ -226,8 +226,9 @@ void vpDisplay::getImage(const vpImage &Isrc, vpImage &Idest) */ void vpDisplay::setDownScalingFactor(unsigned int scale) { - if (!m_displayHasBeenInitialized) + if (!m_displayHasBeenInitialized) { m_scale = scale; + } else { std::cout << "Warning: Cannot apply the down scaling factor " << scale << " to the display window since the display is initialized yet..." << std::endl; @@ -243,9 +244,9 @@ unsigned int vpDisplay::computeAutoScale(unsigned int width, unsigned int height { unsigned int screen_width, screen_height; getScreenSize(screen_width, screen_height); - double wscale = std::max(1., ceil(2. * (double)width / (double)screen_width)); - double hscale = std::max(1., ceil(2. * (double)height / (double)screen_height)); - unsigned int scale = std::max(1u, std::max((unsigned int)wscale, (unsigned int)hscale)); + double wscale = std::max(1., ceil(2. * static_cast(width) / static_cast(screen_width))); + double hscale = std::max(1., ceil(2. * static_cast(height) / static_cast(screen_height))); + unsigned int scale = std::max(1u, std::max(static_cast(wscale), static_cast(hscale))); return scale; } @@ -319,6 +320,7 @@ int main() */ void vpDisplay::setDownScalingFactor(vpScaleType scaleType) { - if (!m_displayHasBeenInitialized) + if (!m_displayHasBeenInitialized) { m_scaleType = scaleType; + } } diff --git a/modules/core/src/display/vpDisplay_impl.h b/modules/core/src/display/vpDisplay_impl.h index 2fb96b3489..253027b7ac 100644 --- a/modules/core/src/display/vpDisplay_impl.h +++ b/modules/core/src/display/vpDisplay_impl.h @@ -88,13 +88,13 @@ void vp_display_display_camera(const vpImage &I, const vpHomogeneousMatrix pt[3].setWorldCoordinates(-halfSize, halfSize, 0.0); pt[4].setWorldCoordinates(0.0, 0.0, -size); - for (int i = 0; i < 5; i++) + for (int i = 0; i < 5; ++i) pt[i].track(cMo); vpImagePoint ip, ip_1, ip0; vpMeterPixelConversion::convertPoint(cam, pt[4].p[0], pt[4].p[1], ip0); - for (int i = 0; i < 4; i++) { + for (int i = 0; i < 4; ++i) { vpMeterPixelConversion::convertPoint(cam, pt[i].p[0], pt[i].p[1], ip_1); vpMeterPixelConversion::convertPoint(cam, pt[(i + 1) % 4].p[0], pt[(i + 1) % 4].p[1], ip); vpDisplay::displayLine(I, ip_1, ip, color, thickness); @@ -267,7 +267,7 @@ void vp_display_display_ellipse(const vpImage &I, const vpImagePoint ¢ } // display the arc of the ellipse by successive small segments - for (unsigned int i = 0; i < nbpoints; i++) { + for (unsigned int i = 0; i < nbpoints; ++i) { angle += incr; // Two concentric circles method used u = a * cos(angle); @@ -352,7 +352,7 @@ void vp_display_display_frame(const vpImage &I, const vpHomogeneousMatrix double abs_v_val = std::abs(cMo[1][0]); // Taking the X-axis of the object frame as first initial guess int v_direction = vpMath::sign(cMo[1][0]); // Taking the direction of the X-axis of the object frame in the camera frame - for (int i = 1; i <= 2; i++) { + for (int i = 1; i <= 2; ++i) { double abs_u_candidate = std::abs(cMo[0][i]); int u_direction_candidate = vpMath::sign(cMo[0][i]); if (abs_u_candidate - abs_u_val > std::numeric_limits::epsilon()) { @@ -444,12 +444,12 @@ void vp_display_display_polygon(const vpImage &I, const std::vectordisplayLine(vip[i], vip[(i + 1) % vip.size()], color, thickness); } } else { - for (unsigned int i = 1; i < vip.size(); i++) { + for (unsigned int i = 1; i < vip.size(); ++i) { (I.display)->displayLine(vip[i - 1], vip[i], color, thickness); } } diff --git a/modules/core/src/display/vpDisplay_rgba.cpp b/modules/core/src/display/vpDisplay_rgba.cpp index a35cbbfa9a..92deb8637a 100644 --- a/modules/core/src/display/vpDisplay_rgba.cpp +++ b/modules/core/src/display/vpDisplay_rgba.cpp @@ -255,14 +255,17 @@ void vpDisplay::displayDotLine(const vpImage &I, int i1, int j1, int i2, void vpDisplay::displayDotLine(const vpImage &I, const std::vector &ips, bool closeTheShape, const vpColor &color, unsigned int thickness) { - if (ips.size() <= 1) + if (ips.size() <= 1) { return; + } - for (size_t i = 0; i < ips.size() - 1; i++) + for (size_t i = 0; i < ips.size() - 1; ++i) { vp_display_display_dot_line(I, ips[i], ips[i + 1], color, thickness); + } - if (closeTheShape) + if (closeTheShape) { vp_display_display_dot_line(I, ips.front(), ips.back(), color, thickness); + } } /*! @@ -277,12 +280,13 @@ void vpDisplay::displayDotLine(const vpImage &I, const std::vector &I, const std::list &ips, bool closeTheShape, const vpColor &color, unsigned int thickness) { - if (ips.size() <= 1) + if (ips.size() <= 1) { return; + } std::list::const_iterator it = ips.begin(); - vpImagePoint ip_prev = *(it++); + vpImagePoint ip_prev = *(++it); for (; it != ips.end(); ++it) { if (vpImagePoint::distance(ip_prev, *it) > 1) { vp_display_display_dot_line(I, ip_prev, *it, color, thickness); @@ -457,16 +461,16 @@ void vpDisplay::displayLine(const vpImage &I, int i1, int j1, int i2, in } else { double a = delta_i / delta_j; - double b = static_cast(i1) - a * static_cast(j1); + double b = static_cast(i1) - (a * static_cast(j1)); std::vector vip; // Image points that intersect image borders // Test intersection with vertical line j=0 vpImagePoint ip_left(b, 0); - if (ip_left.get_i() >= 0. && ip_left.get_i() <= (I.getHeight() - 1.)) { + if ((ip_left.get_i() >= 0.) && (ip_left.get_i() <= (I.getHeight() - 1.))) { vip.push_back(ip_left); } // Test intersection with vertical line j=width-1 - vpImagePoint ip_right(a * (I.getWidth() - 1) + b, I.getWidth() - 1.); - if (ip_right.get_i() >= 0. && ip_right.get_i() <= (I.getHeight() - 1.)) { + vpImagePoint ip_right((a * (I.getWidth() - 1)) + b, I.getWidth() - 1.); + if ((ip_right.get_i() >= 0.) && (ip_right.get_i() <= (I.getHeight() - 1.))) { vip.push_back(ip_right); } if (vip.size() == 2) { @@ -475,7 +479,7 @@ void vpDisplay::displayLine(const vpImage &I, int i1, int j1, int i2, in } // Test intersection with horizontal line i=0 vpImagePoint ip_top(0, -b / a); - if (ip_top.get_j() >= 0. && ip_top.get_j() <= (I.getWidth() - 1.)) { + if ((ip_top.get_j() >= 0.) && (ip_top.get_j() <= (I.getWidth() - 1.))) { vip.push_back(ip_top); } if (vip.size() == 2) { @@ -484,7 +488,7 @@ void vpDisplay::displayLine(const vpImage &I, int i1, int j1, int i2, in } // Test intersection with horizontal line i=height-1 vpImagePoint ip_bottom(I.getHeight() - 1., (I.getHeight() - 1. - b) / a); - if (ip_bottom.get_j() >= 0. && ip_bottom.get_j() <= (I.getWidth() - 1.)) { + if ((ip_bottom.get_j() >= 0.) && (ip_bottom.get_j() <= (I.getWidth() - 1.))) { vip.push_back(ip_bottom); } if (vip.size() == 2) { @@ -507,14 +511,17 @@ void vpDisplay::displayLine(const vpImage &I, int i1, int j1, int i2, in void vpDisplay::displayLine(const vpImage &I, const std::vector &ips, bool closeTheShape, const vpColor &color, unsigned int thickness) { - if (ips.size() <= 1) + if (ips.size() <= 1) { return; + } - for (size_t i = 0; i < ips.size() - 1; i++) + for (size_t i = 0; i < ips.size() - 1; ++i) { vp_display_display_line(I, ips[i], ips[i + 1], color, thickness); + } - if (closeTheShape) + if (closeTheShape) { vp_display_display_line(I, ips.front(), ips.back(), color, thickness); + } } /*! @@ -529,12 +536,13 @@ void vpDisplay::displayLine(const vpImage &I, const std::vector &I, const std::list &ips, bool closeTheShape, const vpColor &color, unsigned int thickness) { - if (ips.size() <= 1) + if (ips.size() <= 1) { return; + } std::list::const_iterator it = ips.begin(); - vpImagePoint ip_prev = *(it++); + vpImagePoint ip_prev = *(++it); for (; it != ips.end(); ++it) { if (vpImagePoint::distance(ip_prev, *it) > 1) { vp_display_display_line(I, ip_prev, *it, color, thickness); diff --git a/modules/core/src/display/vpDisplay_uchar.cpp b/modules/core/src/display/vpDisplay_uchar.cpp index 9206db101f..26479195e3 100644 --- a/modules/core/src/display/vpDisplay_uchar.cpp +++ b/modules/core/src/display/vpDisplay_uchar.cpp @@ -256,14 +256,17 @@ void vpDisplay::displayDotLine(const vpImage &I, int i1, int j1, void vpDisplay::displayDotLine(const vpImage &I, const std::vector &ips, bool closeTheShape, const vpColor &color, unsigned int thickness) { - if (ips.size() <= 1) + if (ips.size() <= 1) { return; + } - for (size_t i = 0; i < ips.size() - 1; i++) + for (size_t i = 0; i < ips.size() - 1; ++i) { vp_display_display_dot_line(I, ips[i], ips[i + 1], color, thickness); + } - if (closeTheShape) + if (closeTheShape) { vp_display_display_dot_line(I, ips.front(), ips.back(), color, thickness); + } } /*! @@ -278,12 +281,13 @@ void vpDisplay::displayDotLine(const vpImage &I, const std::vecto void vpDisplay::displayDotLine(const vpImage &I, const std::list &ips, bool closeTheShape, const vpColor &color, unsigned int thickness) { - if (ips.size() <= 1) + if (ips.size() <= 1) { return; + } std::list::const_iterator it = ips.begin(); - vpImagePoint ip_prev = *(it++); + vpImagePoint ip_prev = *(++it); for (; it != ips.end(); ++it) { if (vpImagePoint::distance(ip_prev, *it) > 1) { vp_display_display_dot_line(I, ip_prev, *it, color, thickness); @@ -458,7 +462,7 @@ void vpDisplay::displayLine(const vpImage &I, int i1, int j1, int } else { double a = delta_i / delta_j; - double b = static_cast(i1) - a * static_cast(j1); + double b = static_cast(i1) - (a * static_cast(j1)); std::vector vip; // Image points that intersect image borders // Test intersection with vertical line j=0 vpImagePoint ip_left(b, 0); @@ -467,7 +471,7 @@ void vpDisplay::displayLine(const vpImage &I, int i1, int j1, int } // Test intersection with vertical line j=width-1 vpImagePoint ip_right(a * (I.getWidth() - 1) + b, I.getWidth() - 1.); - if (ip_right.get_i() >= 0. && ip_right.get_i() <= (I.getHeight() - 1.)) { + if ((ip_right.get_i() >= 0.) && (ip_right.get_i() <= (I.getHeight() - 1.))) { vip.push_back(ip_right); } if (vip.size() == 2) { @@ -476,7 +480,7 @@ void vpDisplay::displayLine(const vpImage &I, int i1, int j1, int } // Test intersection with horizontal line i=0 vpImagePoint ip_top(0, -b / a); - if (ip_top.get_j() >= 0. && ip_top.get_j() <= (I.getWidth() - 1.)) { + if ((ip_top.get_j() >= 0.) && (ip_top.get_j() <= (I.getWidth() - 1.))) { vip.push_back(ip_top); } if (vip.size() == 2) { @@ -485,7 +489,7 @@ void vpDisplay::displayLine(const vpImage &I, int i1, int j1, int } // Test intersection with horizontal line i=height-1 vpImagePoint ip_bottom(I.getHeight() - 1., (I.getHeight() - 1. - b) / a); - if (ip_bottom.get_j() >= 0. && ip_bottom.get_j() <= (I.getWidth() - 1.)) { + if ((ip_bottom.get_j() >= 0.) && (ip_bottom.get_j() <= (I.getWidth() - 1.))) { vip.push_back(ip_bottom); } if (vip.size() == 2) { @@ -507,14 +511,17 @@ void vpDisplay::displayLine(const vpImage &I, int i1, int j1, int void vpDisplay::displayLine(const vpImage &I, const std::vector &ips, bool closeTheShape, const vpColor &color, unsigned int thickness) { - if (ips.size() <= 1) + if (ips.size() <= 1) { return; + } - for (size_t i = 0; i < ips.size() - 1; i++) + for (size_t i = 0; i < ips.size() - 1; ++i) { vp_display_display_line(I, ips[i], ips[i + 1], color, thickness); + } - if (closeTheShape) + if (closeTheShape) { vp_display_display_line(I, ips.front(), ips.back(), color, thickness); + } } /*! @@ -528,12 +535,13 @@ void vpDisplay::displayLine(const vpImage &I, const std::vector &I, const std::list &ips, bool closeTheShape, const vpColor &color, unsigned int thickness) { - if (ips.size() <= 1) + if (ips.size() <= 1) { return; + } std::list::const_iterator it = ips.begin(); - vpImagePoint ip_prev = *(it++); + vpImagePoint ip_prev = *(++it); for (; it != ips.end(); ++it) { if (vpImagePoint::distance(ip_prev, *it) > 1) { vp_display_display_line(I, ip_prev, *it, color, thickness); diff --git a/modules/core/src/display/vpFeatureDisplay.cpp b/modules/core/src/display/vpFeatureDisplay.cpp index 00af61dde8..ed39ffa9b5 100644 --- a/modules/core/src/display/vpFeatureDisplay.cpp +++ b/modules/core/src/display/vpFeatureDisplay.cpp @@ -81,11 +81,11 @@ void vpFeatureDisplay::displayPoint(double x, double y, const vpCameraParameters void vpFeatureDisplay::displayLine(double rho, double theta, const vpCameraParameters &cam, const vpImage &I, const vpColor &color, unsigned int thickness) { - // x cos(theta) + y sin(theta) - rho = 0 + // --comment: x cos(theta) + y sin(theta) - rho = 0 double rhop, thetap; vpMeterPixelConversion::convertLine(cam, rho, theta, rhop, thetap); - // u cos(thetap) + v sin(thetap) - rhop = 0 + // --comment: u cos(thetap) + v sin(thetap) - rhop = 0 double co = cos(thetap); double si = sin(thetap); double c = -rhop; @@ -97,12 +97,12 @@ void vpFeatureDisplay::displayLine(double rho, double theta, const vpCameraParam if (fabs(a) < fabs(b)) { ip1.set_ij(0, (-c) / b); double h = I.getHeight() - 1; - ip2.set_ij(h, (-c - a * h) / b); + ip2.set_ij(h, (-c - (a * h)) / b); vpDisplay::displayLine(I, ip1, ip2, color, thickness); } else { ip1.set_ij((-c) / a, 0); double w = I.getWidth() - 1; - ip2.set_ij((-c - b * w) / a, w); + ip2.set_ij((-c - (b * w)) / a, w); vpDisplay::displayLine(I, ip1, ip2, color, thickness); } } @@ -193,11 +193,11 @@ void vpFeatureDisplay::displayPoint(double x, double y, const vpCameraParameters void vpFeatureDisplay::displayLine(double rho, double theta, const vpCameraParameters &cam, const vpImage &I, const vpColor &color, unsigned int thickness) { - // x cos(theta) + y sin(theta) - rho = 0 + // --comment: x cos(theta) + y sin(theta) - rho = 0 double rhop, thetap; vpMeterPixelConversion::convertLine(cam, rho, theta, rhop, thetap); - // u cos(thetap) + v sin(thetap) - rhop = 0 + // --comment: u cos(thetap) + v sin(thetap) - rhop = 0 double co = cos(thetap); double si = sin(thetap); double c = -rhop; @@ -209,12 +209,12 @@ void vpFeatureDisplay::displayLine(double rho, double theta, const vpCameraParam if (fabs(a) < fabs(b)) { ip1.set_ij(0, (-c) / b); double h = I.getHeight() - 1; - ip2.set_ij(h, (-c - a * h) / b); + ip2.set_ij(h, (-c - (a * h)) / b); vpDisplay::displayLine(I, ip1, ip2, color, thickness); } else { ip1.set_ij((-c) / a, 0); double w = I.getWidth() - 1; - ip2.set_ij((-c - b * w) / a, w); + ip2.set_ij((-c - (b * w)) / a, w); vpDisplay::displayLine(I, ip1, ip2, color, thickness); } } diff --git a/modules/core/src/image/private/vpBayerConversion.h b/modules/core/src/image/private/vpBayerConversion.h index af9dd9e410..3cce883e44 100644 --- a/modules/core/src/image/private/vpBayerConversion.h +++ b/modules/core/src/image/private/vpBayerConversion.h @@ -140,7 +140,7 @@ void demosaicBGGRToRGBaBilinearTpl(const T *bggr, T *rgba, unsigned int width, u rgba[((height - 1) * width + width - 1) * 4 + 2] = bggr[(height - 1) * width - 2]; // i == 0 - for (unsigned int j = 1; j < width - 1; j++) { + for (unsigned int j = 1; j < width - 1; ++j) { if (j % 2 == 0) { rgba[j * 4 + 0] = static_cast(0.5f * bggr[width + j - 1] + 0.5f * bggr[width + j + 1]); rgba[j * 4 + 1] = static_cast(0.5f * bggr[j - 1] + 0.5f * bggr[j + 1]); @@ -154,7 +154,7 @@ void demosaicBGGRToRGBaBilinearTpl(const T *bggr, T *rgba, unsigned int width, u } // j == 0 - for (unsigned int i = 1; i < height - 1; i++) { + for (unsigned int i = 1; i < height - 1; ++i) { if (i % 2 == 0) { rgba[i * width * 4 + 0] = static_cast(0.5f * bggr[(i - 1) * width + 1] + 0.5f * bggr[(i + 1) * width + 1]); rgba[i * width * 4 + 1] = bggr[i * width + 1]; @@ -168,7 +168,7 @@ void demosaicBGGRToRGBaBilinearTpl(const T *bggr, T *rgba, unsigned int width, u } // j == width-1 - for (unsigned int i = 1; i < height - 1; i++) { + for (unsigned int i = 1; i < height - 1; ++i) { if (i % 2 == 0) { rgba[(i * width + width - 1) * 4 + 0] = static_cast(0.5f * bggr[i * width - 1] + 0.5f * bggr[(i + 2) * width - 1]); @@ -184,7 +184,7 @@ void demosaicBGGRToRGBaBilinearTpl(const T *bggr, T *rgba, unsigned int width, u } // i == height-1 - for (unsigned int j = 1; j < width - 1; j++) { + for (unsigned int j = 1; j < width - 1; ++j) { if (j % 2 == 0) { rgba[((height - 1) * width + j) * 4 + 0] = static_cast(0.5f * bggr[(height - 1) * width + j - 1] + 0.5f * bggr[(height - 1) * width + j + 1]); @@ -208,8 +208,8 @@ void demosaicBGGRToRGBaBilinearTpl(const T *bggr, T *rgba, unsigned int width, u #else (void)nThreads; #endif - for (unsigned int i = 1; i < height - 1; i++) { - for (unsigned int j = 1; j < width - 1; j++) { + for (unsigned int i = 1; i < height - 1; ++i) { + for (unsigned int j = 1; j < width - 1; ++j) { if (i % 2 == 0 && j % 2 == 0) { rgba[(i * width + j) * 4 + 0] = demosaicCheckerBilinear(bggr, width, i, j); rgba[(i * width + j) * 4 + 1] = demosaicCrossBilinear(bggr, width, i, j); @@ -264,7 +264,7 @@ void demosaicGBRGToRGBaBilinearTpl(const T *gbrg, T *rgba, unsigned int width, u rgba[((height - 1) * width + width - 1) * 4 + 2] = gbrg[(height - 1) * width - 1]; // i == 0 - for (unsigned int j = 1; j < width - 1; j++) { + for (unsigned int j = 1; j < width - 1; ++j) { if (j % 2 == 0) { rgba[j * 4 + 0] = gbrg[width + j]; rgba[j * 4 + 1] = gbrg[j]; @@ -278,7 +278,7 @@ void demosaicGBRGToRGBaBilinearTpl(const T *gbrg, T *rgba, unsigned int width, u } // j == 0 - for (unsigned int i = 1; i < height - 1; i++) { + for (unsigned int i = 1; i < height - 1; ++i) { if (i % 2 == 0) { rgba[i * width * 4 + 0] = static_cast(0.5f * gbrg[(i - 1) * width] + 0.5f * gbrg[(i + 1) * width]); rgba[i * width * 4 + 1] = gbrg[i * width]; @@ -292,7 +292,7 @@ void demosaicGBRGToRGBaBilinearTpl(const T *gbrg, T *rgba, unsigned int width, u } // j == width-1 - for (unsigned int i = 1; i < height - 1; i++) { + for (unsigned int i = 1; i < height - 1; ++i) { if (i % 2 == 0) { rgba[(i * width + width - 1) * 4 + 0] = static_cast(0.5f * gbrg[i * width - 2] + 0.5f * gbrg[(i + 2) * width - 2]); @@ -308,7 +308,7 @@ void demosaicGBRGToRGBaBilinearTpl(const T *gbrg, T *rgba, unsigned int width, u } // i == height-1 - for (unsigned int j = 1; j < width - 1; j++) { + for (unsigned int j = 1; j < width - 1; ++j) { if (j % 2 == 0) { rgba[((height - 1) * width + j) * 4 + 0] = gbrg[(height - 1) * width + j]; rgba[((height - 1) * width + j) * 4 + 1] = @@ -332,8 +332,8 @@ void demosaicGBRGToRGBaBilinearTpl(const T *gbrg, T *rgba, unsigned int width, u #else (void)nThreads; #endif - for (unsigned int i = 1; i < height - 1; i++) { - for (unsigned int j = 1; j < width - 1; j++) { + for (unsigned int i = 1; i < height - 1; ++i) { + for (unsigned int j = 1; j < width - 1; ++j) { if (i % 2 == 0 && j % 2 == 0) { rgba[(i * width + j) * 4 + 0] = demosaicPhiBilinear(gbrg, width, i, j); rgba[(i * width + j) * 4 + 1] = gbrg[i * width + j]; @@ -388,7 +388,7 @@ void demosaicGRBGToRGBaBilinearTpl(const T *grbg, T *rgba, unsigned int width, u rgba[((height - 1) * width + width - 1) * 4 + 2] = grbg[height * width - 2]; // i == 0 - for (unsigned int j = 1; j < width - 1; j++) { + for (unsigned int j = 1; j < width - 1; ++j) { if (j % 2 == 0) { rgba[j * 4 + 0] = static_cast(0.5f * grbg[j - 1] + 0.5f * grbg[j + 1]); rgba[j * 4 + 1] = grbg[j]; @@ -402,7 +402,7 @@ void demosaicGRBGToRGBaBilinearTpl(const T *grbg, T *rgba, unsigned int width, u } // j == 0 - for (unsigned int i = 1; i < height - 1; i++) { + for (unsigned int i = 1; i < height - 1; ++i) { if (i % 2 == 0) { rgba[i * width * 4 + 0] = grbg[i * width + 1]; rgba[i * width * 4 + 1] = grbg[i * width]; @@ -416,7 +416,7 @@ void demosaicGRBGToRGBaBilinearTpl(const T *grbg, T *rgba, unsigned int width, u } // j == width-1 - for (unsigned int i = 1; i < height - 1; i++) { + for (unsigned int i = 1; i < height - 1; ++i) { if (i % 2 == 0) { rgba[(i * width + width - 1) * 4 + 0] = grbg[(i + 1) * width - 1]; rgba[(i * width + width - 1) * 4 + 1] = grbg[(i + 1) * width - 2]; @@ -432,7 +432,7 @@ void demosaicGRBGToRGBaBilinearTpl(const T *grbg, T *rgba, unsigned int width, u } // i == height-1 - for (unsigned int j = 1; j < width - 1; j++) { + for (unsigned int j = 1; j < width - 1; ++j) { if (j % 2 == 0) { rgba[((height - 1) * width + j) * 4 + 0] = static_cast(0.5f * grbg[(height - 2) * width + j - 1] + 0.5f * grbg[(height - 2) * width + j + 1]); @@ -456,8 +456,8 @@ void demosaicGRBGToRGBaBilinearTpl(const T *grbg, T *rgba, unsigned int width, u #else (void)nThreads; #endif - for (unsigned int i = 1; i < height - 1; i++) { - for (unsigned int j = 1; j < width - 1; j++) { + for (unsigned int i = 1; i < height - 1; ++i) { + for (unsigned int j = 1; j < width - 1; ++j) { if (i % 2 == 0 && j % 2 == 0) { rgba[(i * width + j) * 4 + 0] = demosaicThetaBilinear(grbg, width, i, j); rgba[(i * width + j) * 4 + 1] = grbg[i * width + j]; @@ -512,7 +512,7 @@ void demosaicRGGBToRGBaBilinearTpl(const T *rggb, T *rgba, unsigned int width, u rgba[((height - 1) * width + width - 1) * 4 + 2] = rggb[height * width - 1]; // i == 0 - for (unsigned int j = 1; j < width - 1; j++) { + for (unsigned int j = 1; j < width - 1; ++j) { if (j % 2 == 0) { rgba[j * 4 + 0] = rggb[j]; rgba[j * 4 + 1] = static_cast(0.5f * rggb[j - 1] + 0.5f * rggb[j + 1]); @@ -526,7 +526,7 @@ void demosaicRGGBToRGBaBilinearTpl(const T *rggb, T *rgba, unsigned int width, u } // j == 0 - for (unsigned int i = 1; i < height - 1; i++) { + for (unsigned int i = 1; i < height - 1; ++i) { if (i % 2 == 0) { rgba[i * width * 4 + 0] = rggb[i * width]; rgba[i * width * 4 + 1] = rggb[i * width + 1]; @@ -540,7 +540,7 @@ void demosaicRGGBToRGBaBilinearTpl(const T *rggb, T *rgba, unsigned int width, u } // j == width-1 - for (unsigned int i = 1; i < height - 1; i++) { + for (unsigned int i = 1; i < height - 1; ++i) { if (i % 2 == 0) { rgba[(i * width + width - 1) * 4 + 0] = rggb[(i + 1) * width - 2]; rgba[(i * width + width - 1) * 4 + 1] = rggb[(i + 1) * width - 1]; @@ -556,7 +556,7 @@ void demosaicRGGBToRGBaBilinearTpl(const T *rggb, T *rgba, unsigned int width, u } // i == height-1 - for (unsigned int j = 1; j < width - 1; j++) { + for (unsigned int j = 1; j < width - 1; ++j) { if (j % 2 == 0) { rgba[((height - 1) * width + j) * 4 + 0] = rggb[(height - 2) * width + j]; rgba[((height - 1) * width + j) * 4 + 1] = rggb[(height - 1) * width + j]; @@ -580,8 +580,8 @@ void demosaicRGGBToRGBaBilinearTpl(const T *rggb, T *rgba, unsigned int width, u #else (void)nThreads; #endif - for (unsigned int i = 1; i < height - 1; i++) { - for (unsigned int j = 1; j < width - 1; j++) { + for (unsigned int i = 1; i < height - 1; ++i) { + for (unsigned int j = 1; j < width - 1; ++j) { if (i % 2 == 0 && j % 2 == 0) { rgba[(i * width + j) * 4 + 0] = rggb[i * width + j]; rgba[(i * width + j) * 4 + 1] = demosaicCrossBilinear(rggb, width, i, j); @@ -637,7 +637,7 @@ void demosaicBGGRToRGBaMalvarTpl(const T *bggr, T *rgba, unsigned int width, uns rgba[((height - 1) * width + width - 1) * 4 + 2] = bggr[(height - 1) * width - 2]; // i == 0 - for (unsigned int j = 1; j < width - 1; j++) { + for (unsigned int j = 1; j < width - 1; ++j) { if (j % 2 == 0) { rgba[j * 4 + 0] = static_cast(0.5f * bggr[width + j - 1] + 0.5f * bggr[width + j + 1]); rgba[j * 4 + 1] = static_cast(0.5f * bggr[j - 1] + 0.5f * bggr[j + 1]); @@ -651,7 +651,7 @@ void demosaicBGGRToRGBaMalvarTpl(const T *bggr, T *rgba, unsigned int width, uns } // i == 1 - for (unsigned int j = 1; j < width - 1; j++) { + for (unsigned int j = 1; j < width - 1; ++j) { if (j % 2 == 0) { rgba[(width + j) * 4 + 0] = static_cast(0.5f * bggr[width + j - 1] + 0.5f * bggr[width + j + 1]); rgba[(width + j) * 4 + 1] = bggr[width + j]; @@ -667,7 +667,7 @@ void demosaicBGGRToRGBaMalvarTpl(const T *bggr, T *rgba, unsigned int width, uns } // j == 0 - for (unsigned int i = 1; i < height - 1; i++) { + for (unsigned int i = 1; i < height - 1; ++i) { if (i % 2 == 0) { rgba[i * width * 4 + 0] = static_cast(0.5f * bggr[(i - 1) * width + 1] + 0.5f * bggr[(i + 1) * width + 1]); rgba[i * width * 4 + 1] = bggr[i * width + 1]; @@ -681,7 +681,7 @@ void demosaicBGGRToRGBaMalvarTpl(const T *bggr, T *rgba, unsigned int width, uns } // j == 1 - for (unsigned int i = 1; i < height - 1; i++) { + for (unsigned int i = 1; i < height - 1; ++i) { if (i % 2 == 0) { rgba[(i * width + 1) * 4 + 0] = static_cast(0.5f * bggr[(i - 1) * width + 1] + 0.5f * bggr[(i + 1) * width + 1]); @@ -698,7 +698,7 @@ void demosaicBGGRToRGBaMalvarTpl(const T *bggr, T *rgba, unsigned int width, uns } // j == width-2 - for (unsigned int i = 1; i < height - 1; i++) { + for (unsigned int i = 1; i < height - 1; ++i) { if (i % 2 == 0) { rgba[(i * width + width - 2) * 4 + 0] = static_cast(0.25f * bggr[i * width - 3] + 0.25f * bggr[i * width - 1] + 0.25f * bggr[(i + 2) * width - 3] + @@ -718,7 +718,7 @@ void demosaicBGGRToRGBaMalvarTpl(const T *bggr, T *rgba, unsigned int width, uns } // j == width-1 - for (unsigned int i = 1; i < height - 1; i++) { + for (unsigned int i = 1; i < height - 1; ++i) { if (i % 2 == 0) { rgba[(i * width + width - 1) * 4 + 0] = static_cast(0.5f * bggr[i * width - 1] + 0.5f * bggr[(i + 2) * width - 1]); @@ -734,7 +734,7 @@ void demosaicBGGRToRGBaMalvarTpl(const T *bggr, T *rgba, unsigned int width, uns } // i == height-2 - for (unsigned int j = 1; j < width - 1; j++) { + for (unsigned int j = 1; j < width - 1; ++j) { if (j % 2 == 0) { rgba[((height - 2) * width + j) * 4 + 0] = static_cast(0.25f * bggr[(height - 3) * width + j - 1] + 0.25f * bggr[(height - 3) * width + j + 1] + @@ -753,7 +753,7 @@ void demosaicBGGRToRGBaMalvarTpl(const T *bggr, T *rgba, unsigned int width, uns } // i == height-1 - for (unsigned int j = 1; j < width - 1; j++) { + for (unsigned int j = 1; j < width - 1; ++j) { if (j % 2 == 0) { rgba[((height - 1) * width + j) * 4 + 0] = static_cast(0.5f * bggr[(height - 1) * width + j - 1] + 0.5f * bggr[(height - 1) * width + j + 1]); @@ -777,8 +777,8 @@ void demosaicBGGRToRGBaMalvarTpl(const T *bggr, T *rgba, unsigned int width, uns #else (void)nThreads; #endif - for (unsigned int i = 2; i < height - 2; i++) { - for (unsigned int j = 2; j < width - 2; j++) { + for (unsigned int i = 2; i < height - 2; ++i) { + for (unsigned int j = 2; j < width - 2; ++j) { if (i % 2 == 0 && j % 2 == 0) { rgba[(i * width + j) * 4 + 0] = demosaicCheckerMalvar(bggr, width, i, j); rgba[(i * width + j) * 4 + 1] = demosaicCrossMalvar(bggr, width, i, j); @@ -832,7 +832,7 @@ void demosaicGBRGToRGBaMalvarTpl(const T *gbrg, T *rgba, unsigned int width, uns rgba[((height - 1) * width + width - 1) * 4 + 2] = gbrg[(height - 1) * width - 1]; // i == 0 - for (unsigned int j = 1; j < width - 1; j++) { + for (unsigned int j = 1; j < width - 1; ++j) { if (j % 2 == 0) { rgba[j * 4 + 0] = gbrg[width + j]; rgba[j * 4 + 1] = gbrg[j]; @@ -846,7 +846,7 @@ void demosaicGBRGToRGBaMalvarTpl(const T *gbrg, T *rgba, unsigned int width, uns } // i == 1 - for (unsigned int j = 1; j < width - 1; j++) { + for (unsigned int j = 1; j < width - 1; ++j) { if (j % 2 == 0) { rgba[(width + j) * 4 + 0] = gbrg[width + j]; rgba[(width + j) * 4 + 1] = static_cast(0.25f * gbrg[j] + 0.25f * gbrg[width + j - 1] + @@ -862,7 +862,7 @@ void demosaicGBRGToRGBaMalvarTpl(const T *gbrg, T *rgba, unsigned int width, uns } // j == 0 - for (unsigned int i = 1; i < height - 1; i++) { + for (unsigned int i = 1; i < height - 1; ++i) { if (i % 2 == 0) { rgba[i * width * 4 + 0] = static_cast(0.5f * gbrg[(i - 1) * width] + 0.5f * gbrg[(i + 1) * width]); rgba[i * width * 4 + 1] = gbrg[i * width]; @@ -876,7 +876,7 @@ void demosaicGBRGToRGBaMalvarTpl(const T *gbrg, T *rgba, unsigned int width, uns } // j == 1 - for (unsigned int i = 1; i < height - 1; i++) { + for (unsigned int i = 1; i < height - 1; ++i) { if (i % 2 == 0) { rgba[(i * width + 1) * 4 + 0] = static_cast(0.25f * gbrg[(i - 1) * width] + 0.25f * gbrg[(i - 1) * width + 2] + 0.25f * gbrg[(i + 1) * width] + 0.5f * gbrg[(i + 1) * width + 2]); @@ -893,7 +893,7 @@ void demosaicGBRGToRGBaMalvarTpl(const T *gbrg, T *rgba, unsigned int width, uns } // j == width-2 - for (unsigned int i = 1; i < height - 1; i++) { + for (unsigned int i = 1; i < height - 1; ++i) { if (i % 2 == 0) { rgba[(i * width + width - 2) * 4 + 0] = static_cast(0.5f * gbrg[i * width - 2] + 0.5f * gbrg[(i + 2) * width - 2]); @@ -913,7 +913,7 @@ void demosaicGBRGToRGBaMalvarTpl(const T *gbrg, T *rgba, unsigned int width, uns } // j == width-1 - for (unsigned int i = 1; i < height - 1; i++) { + for (unsigned int i = 1; i < height - 1; ++i) { if (i % 2 == 0) { rgba[(i * width + width - 1) * 4 + 0] = static_cast(0.5f * gbrg[i * width - 2] + 0.5f * gbrg[(i + 2) * width - 2]); @@ -929,7 +929,7 @@ void demosaicGBRGToRGBaMalvarTpl(const T *gbrg, T *rgba, unsigned int width, uns } // i == height-2 - for (unsigned int j = 1; j < width - 1; j++) { + for (unsigned int j = 1; j < width - 1; ++j) { if (j % 2 == 0) { rgba[((height - 2) * width + j) * 4 + 0] = static_cast(0.5f * gbrg[(height - 3) * width + j] + 0.5f * gbrg[(height - 1) * width + j]); @@ -949,7 +949,7 @@ void demosaicGBRGToRGBaMalvarTpl(const T *gbrg, T *rgba, unsigned int width, uns } // i == height-1 - for (unsigned int j = 1; j < width - 1; j++) { + for (unsigned int j = 1; j < width - 1; ++j) { if (j % 2 == 0) { rgba[((height - 1) * width + j) * 4 + 0] = gbrg[(height - 1) * width + j]; rgba[((height - 1) * width + j) * 4 + 1] = @@ -973,8 +973,8 @@ void demosaicGBRGToRGBaMalvarTpl(const T *gbrg, T *rgba, unsigned int width, uns #else (void)nThreads; #endif - for (unsigned int i = 2; i < height - 2; i++) { - for (unsigned int j = 2; j < width - 2; j++) { + for (unsigned int i = 2; i < height - 2; ++i) { + for (unsigned int j = 2; j < width - 2; ++j) { if (i % 2 == 0 && j % 2 == 0) { rgba[(i * width + j) * 4 + 0] = demosaicPhiMalvar(gbrg, width, i, j); rgba[(i * width + j) * 4 + 1] = gbrg[i * width + j]; @@ -1028,7 +1028,7 @@ void demosaicGRBGToRGBaMalvarTpl(const T *grbg, T *rgba, unsigned int width, uns rgba[((height - 1) * width + width - 1) * 4 + 2] = grbg[height * width - 2]; // i == 0 - for (unsigned int j = 1; j < width - 1; j++) { + for (unsigned int j = 1; j < width - 1; ++j) { if (j % 2 == 0) { rgba[j * 4 + 0] = static_cast(0.5f * grbg[j - 1] + 0.5f * grbg[j + 1]); rgba[j * 4 + 1] = grbg[j]; @@ -1042,7 +1042,7 @@ void demosaicGRBGToRGBaMalvarTpl(const T *grbg, T *rgba, unsigned int width, uns } // i == 1 - for (unsigned int j = 1; j < width - 1; j++) { + for (unsigned int j = 1; j < width - 1; ++j) { if (j % 2 == 0) { rgba[(width + j) * 4 + 0] = static_cast(0.25f * grbg[j - 1] + 0.25f * grbg[j + 1] + 0.25f * grbg[2 * width + j - 1] + 0.25f * grbg[2 * width + j + 1]); @@ -1058,7 +1058,7 @@ void demosaicGRBGToRGBaMalvarTpl(const T *grbg, T *rgba, unsigned int width, uns } // j == 0 - for (unsigned int i = 1; i < height - 1; i++) { + for (unsigned int i = 1; i < height - 1; ++i) { if (i % 2 == 0) { rgba[i * width * 4 + 0] = grbg[i * width + 1]; rgba[i * width * 4 + 1] = grbg[i * width]; @@ -1072,7 +1072,7 @@ void demosaicGRBGToRGBaMalvarTpl(const T *grbg, T *rgba, unsigned int width, uns } // j == 1 - for (unsigned int i = 1; i < height - 1; i++) { + for (unsigned int i = 1; i < height - 1; ++i) { if (i % 2 == 0) { rgba[(i * width + 1) * 4 + 0] = grbg[i * width + 1]; rgba[(i * width + 1) * 4 + 1] = static_cast(0.25f * grbg[(i - 1) * width + 1] + 0.25f * grbg[i * width] + @@ -1089,7 +1089,7 @@ void demosaicGRBGToRGBaMalvarTpl(const T *grbg, T *rgba, unsigned int width, uns } // j == width-2 - for (unsigned int i = 1; i < height - 1; i++) { + for (unsigned int i = 1; i < height - 1; ++i) { if (i % 2 == 0) { rgba[(i * width + width - 2) * 4 + 0] = static_cast(0.5f * grbg[(i + 1) * width - 3] + 0.5f * grbg[(i + 1) * width - 1]); @@ -1109,7 +1109,7 @@ void demosaicGRBGToRGBaMalvarTpl(const T *grbg, T *rgba, unsigned int width, uns } // j == width-1 - for (unsigned int i = 1; i < height - 1; i++) { + for (unsigned int i = 1; i < height - 1; ++i) { if (i % 2 == 0) { rgba[(i * width + width - 1) * 4 + 0] = grbg[(i + 1) * width - 1]; rgba[(i * width + width - 1) * 4 + 1] = grbg[(i + 1) * width - 2]; @@ -1125,7 +1125,7 @@ void demosaicGRBGToRGBaMalvarTpl(const T *grbg, T *rgba, unsigned int width, uns } // i == height-2 - for (unsigned int j = 1; j < width - 1; j++) { + for (unsigned int j = 1; j < width - 1; ++j) { if (j % 2 == 0) { rgba[((height - 2) * width + j) * 4 + 0] = static_cast(0.5f * grbg[(height - 2) * width + j - 1] + 0.5f * grbg[(height - 2) * width + j + 1]); @@ -1145,7 +1145,7 @@ void demosaicGRBGToRGBaMalvarTpl(const T *grbg, T *rgba, unsigned int width, uns } // i == height-1 - for (unsigned int j = 1; j < width - 1; j++) { + for (unsigned int j = 1; j < width - 1; ++j) { if (j % 2 == 0) { rgba[((height - 1) * width + j) * 4 + 0] = static_cast(0.5f * grbg[(height - 2) * width + j - 1] + 0.5f * grbg[(height - 2) * width + j + 1]); @@ -1169,8 +1169,8 @@ void demosaicGRBGToRGBaMalvarTpl(const T *grbg, T *rgba, unsigned int width, uns #else (void)nThreads; #endif - for (unsigned int i = 2; i < height - 2; i++) { - for (unsigned int j = 2; j < width - 2; j++) { + for (unsigned int i = 2; i < height - 2; ++i) { + for (unsigned int j = 2; j < width - 2; ++j) { if (i % 2 == 0 && j % 2 == 0) { rgba[(i * width + j) * 4 + 0] = demosaicThetaMalvar(grbg, width, i, j); rgba[(i * width + j) * 4 + 1] = grbg[i * width + j]; @@ -1224,7 +1224,7 @@ void demosaicRGGBToRGBaMalvarTpl(const T *rggb, T *rgba, unsigned int width, uns rgba[((height - 1) * width + width - 1) * 4 + 2] = rggb[height * width - 1]; // i == 0 - for (unsigned int j = 1; j < width - 1; j++) { + for (unsigned int j = 1; j < width - 1; ++j) { if (j % 2 == 0) { rgba[j * 4 + 0] = rggb[j]; rgba[j * 4 + 1] = static_cast(0.5f * rggb[j - 1] + 0.5f * rggb[j + 1]); @@ -1238,7 +1238,7 @@ void demosaicRGGBToRGBaMalvarTpl(const T *rggb, T *rgba, unsigned int width, uns } // i == 1 - for (unsigned int j = 1; j < width - 1; j++) { + for (unsigned int j = 1; j < width - 1; ++j) { if (j % 2 == 0) { rgba[(width + j) * 4 + 0] = static_cast(0.5f * rggb[j] + 0.5f * rggb[2 * width + j]); rgba[(width + j) * 4 + 1] = rggb[width + j]; @@ -1254,7 +1254,7 @@ void demosaicRGGBToRGBaMalvarTpl(const T *rggb, T *rgba, unsigned int width, uns } // j == 0 - for (unsigned int i = 1; i < height - 1; i++) { + for (unsigned int i = 1; i < height - 1; ++i) { if (i % 2 == 0) { rgba[i * width * 4 + 0] = rggb[i * width]; rgba[i * width * 4 + 1] = rggb[i * width + 1]; @@ -1268,7 +1268,7 @@ void demosaicRGGBToRGBaMalvarTpl(const T *rggb, T *rgba, unsigned int width, uns } // j == 1 - for (unsigned int i = 1; i < height - 1; i++) { + for (unsigned int i = 1; i < height - 1; ++i) { if (i % 2 == 0) { rgba[(i * width + 1) * 4 + 0] = static_cast(0.5f * rggb[i * width] + 0.5f * rggb[i * width + 2]); rgba[(i * width + 1) * 4 + 1] = rggb[i * width + 1]; @@ -1285,7 +1285,7 @@ void demosaicRGGBToRGBaMalvarTpl(const T *rggb, T *rgba, unsigned int width, uns } // j == width-2 - for (unsigned int i = 1; i < height - 1; i++) { + for (unsigned int i = 1; i < height - 1; ++i) { if (i % 2 == 0) { rgba[(i * width + width - 2) * 4 + 0] = rggb[(i + 1) * width - 2]; rgba[(i * width + width - 2) * 4 + 1] = @@ -1305,7 +1305,7 @@ void demosaicRGGBToRGBaMalvarTpl(const T *rggb, T *rgba, unsigned int width, uns } // j == width-1 - for (unsigned int i = 1; i < height - 1; i++) { + for (unsigned int i = 1; i < height - 1; ++i) { if (i % 2 == 0) { rgba[(i * width + width - 1) * 4 + 0] = rggb[(i + 1) * width - 2]; rgba[(i * width + width - 1) * 4 + 1] = rggb[(i + 1) * width - 1]; @@ -1321,7 +1321,7 @@ void demosaicRGGBToRGBaMalvarTpl(const T *rggb, T *rgba, unsigned int width, uns } // i == height-2 - for (unsigned int j = 1; j < width - 1; j++) { + for (unsigned int j = 1; j < width - 1; ++j) { if (j % 2 == 0) { rgba[((height - 2) * width + j) * 4 + 0] = rggb[(height - 2) * width + j]; rgba[((height - 2) * width + j) * 4 + 1] = @@ -1341,7 +1341,7 @@ void demosaicRGGBToRGBaMalvarTpl(const T *rggb, T *rgba, unsigned int width, uns } // i == height-1 - for (unsigned int j = 1; j < width - 1; j++) { + for (unsigned int j = 1; j < width - 1; ++j) { if (j % 2 == 0) { rgba[((height - 1) * width + j) * 4 + 0] = rggb[(height - 2) * width + j]; rgba[((height - 1) * width + j) * 4 + 1] = rggb[(height - 1) * width + j]; @@ -1365,8 +1365,8 @@ void demosaicRGGBToRGBaMalvarTpl(const T *rggb, T *rgba, unsigned int width, uns #else (void)nThreads; #endif - for (unsigned int i = 2; i < height - 2; i++) { - for (unsigned int j = 2; j < width - 2; j++) { + for (unsigned int i = 2; i < height - 2; ++i) { + for (unsigned int j = 2; j < width - 2; ++j) { if (i % 2 == 0 && j % 2 == 0) { rgba[(i * width + j) * 4 + 0] = rggb[i * width + j]; rgba[(i * width + j) * 4 + 1] = demosaicCrossMalvar(rggb, width, i, j); diff --git a/modules/core/src/image/private/vpImageConvert_impl.h b/modules/core/src/image/private/vpImageConvert_impl.h index 810caa53fe..da34f3f0a8 100644 --- a/modules/core/src/image/private/vpImageConvert_impl.h +++ b/modules/core/src/image/private/vpImageConvert_impl.h @@ -63,7 +63,7 @@ void vp_createDepthHistogram(const vpImage &src_depth, vpImage= VISP_CXX_STANDARD_11) int nThreads = omp_get_max_threads(); std::vector > histograms(nThreads); - for (int i = 0; i < nThreads; i++) { + for (int i = 0; i < nThreads; ++i) { histograms[i].fill(0); } @@ -75,8 +75,8 @@ void vp_createDepthHistogram(const vpImage &src_depth, vpImage &src_depth, vpImage= VISP_CXX_STANDARD_11) int nThreads = omp_get_max_threads(); std::vector > histograms(nThreads); - for (int i = 0; i < nThreads; i++) { + for (int i = 0; i < nThreads; ++i) { histograms[i].fill(0); } @@ -133,8 +133,8 @@ void vp_createDepthHistogram(const vpImage &src_depth, vpImage= VISP_CXX_STANDARD_11) auto scaleFilter = [](vpArray2D &filter, const float &scale) { - for (unsigned int r = 0; r < filter.getRows(); r++) { - for (unsigned int c = 0; c < filter.getCols(); c++) { + unsigned int filter_rows = filter.getRows(); + unsigned int filter_col = filter.getCols(); + for (unsigned int r = 0; r < filter_rows; ++r) { + for (unsigned int c = 0; c < filter_col; ++c) { filter[r][c] = filter[r][c] * scale; } - }}; + } + }; #endif float scaleX = 1.f; @@ -237,8 +240,8 @@ vpCannyEdgeDetection::detect(const vpImage &I) void vpCannyEdgeDetection::performFilteringAndGradientComputation(const vpImage &I) { - if (m_filteringAndGradientType == vpImageFilter::CANNY_GBLUR_SOBEL_FILTERING - || m_filteringAndGradientType == vpImageFilter::CANNY_GBLUR_SCHARR_FILTERING) { + if ((m_filteringAndGradientType == vpImageFilter::CANNY_GBLUR_SOBEL_FILTERING) + || (m_filteringAndGradientType == vpImageFilter::CANNY_GBLUR_SCHARR_FILTERING)) { // Computing the Gaussian blur vpImage Iblur; vpImage GIx; @@ -284,7 +287,7 @@ getInterpolationWeightsAndOffsets(const float &gradientOrientation, dRowGradAlpha = 0; dRowGradBeta = -1; } - else if (gradientOrientation >= M_PI_4f && gradientOrientation < M_PI_2f) { + else if ((gradientOrientation >= M_PI_4f) && (gradientOrientation < M_PI_2f)) { // Angles between 45 and 90 deg rely on the diagonal and vertical points thetaMin = M_PI_4f; dColGradAlpha = 1; @@ -292,7 +295,7 @@ getInterpolationWeightsAndOffsets(const float &gradientOrientation, dRowGradAlpha = -1; dRowGradBeta = -1; } - else if (gradientOrientation >= M_PI_2f && gradientOrientation < (3.f * M_PI_4f)) { + else if ((gradientOrientation >= M_PI_2f) && (gradientOrientation < (3.f * M_PI_4f))) { // Angles between 90 and 135 deg rely on the vertical and diagonal points thetaMin = M_PI_2f; dColGradAlpha = 0; @@ -300,7 +303,7 @@ getInterpolationWeightsAndOffsets(const float &gradientOrientation, dRowGradAlpha = -1; dRowGradBeta = -1; } - else if (gradientOrientation >= (3.f * M_PI_4f) && gradientOrientation < M_PIf) { + else if ((gradientOrientation >= (3.f * M_PI_4f)) && (gradientOrientation < M_PIf)) { // Angles between 135 and 180 deg rely on the vertical and diagonal points thetaMin = 3.f * M_PI_4f; dColGradAlpha = -1; @@ -327,10 +330,10 @@ getManhattanGradient(const vpImage &dIx, const vpImage &dIy, const float grad = 0.; int nbRows = dIx.getRows(); int nbCols = dIx.getCols(); - if (row >= 0 - && row < nbRows - && col >= 0 - && col < nbCols + if ((row >= 0) + && (row < nbRows) + && (col >= 0) + && (col < nbCols) ) { float dx = dIx[row][col]; float dy = dIy[row][col]; @@ -377,8 +380,8 @@ vpCannyEdgeDetection::performEdgeThinning(const float &lowerThreshold) int nbRows = m_dIx.getRows(); int nbCols = m_dIx.getCols(); - for (int row = 0; row < nbRows; row++) { - for (int col = 0; col < nbCols; col++) { + for (int row = 0; row < nbRows; ++row) { + for (int col = 0; col < nbCols; ++col) { if (mp_mask != nullptr) { if (!(*mp_mask)[row][col]) { // The mask tells us to ignore the current pixel @@ -407,10 +410,10 @@ vpCannyEdgeDetection::performEdgeThinning(const float &lowerThreshold) float gradBetaPlus = getManhattanGradient(m_dIx, m_dIy, row + dRowBetaPlus, col + dColBetaPlus); float gradAlphaMinus = getManhattanGradient(m_dIx, m_dIy, row + dRowAlphaMinus, col + dColAphaMinus); float gradBetaMinus = getManhattanGradient(m_dIx, m_dIy, row + dRowBetaMinus, col + dColBetaMinus); - float gradPlus = alpha * gradAlphaPlus + beta * gradBetaPlus; - float gradMinus = alpha * gradAlphaMinus + beta * gradBetaMinus; + float gradPlus = (alpha * gradAlphaPlus) + (beta * gradBetaPlus); + float gradMinus = (alpha * gradAlphaMinus) + (beta * gradBetaMinus); - if (grad >= gradPlus && grad >= gradMinus) { + if ((grad >= gradPlus) && (grad >= gradMinus)) { // Keeping the edge point that has the highest gradient std::pair bestPixel(row, col); m_edgeCandidateAndGradient[bestPixel] = grad; @@ -423,11 +426,11 @@ void vpCannyEdgeDetection::performHysteresisThresholding(const float &lowerThreshold, const float &upperThreshold) { std::map, float>::iterator it; - for (it = m_edgeCandidateAndGradient.begin(); it != m_edgeCandidateAndGradient.end(); it++) { + for (it = m_edgeCandidateAndGradient.begin(); it != m_edgeCandidateAndGradient.end(); ++it) { if (it->second >= upperThreshold) { m_edgePointsCandidates[it->first] = STRONG_EDGE; } - else if (it->second >= lowerThreshold && it->second < upperThreshold) { + else if ((it->second >= lowerThreshold) && (it->second < upperThreshold)) { m_edgePointsCandidates[it->first] = WEAK_EDGE; } } @@ -437,7 +440,7 @@ void vpCannyEdgeDetection::performEdgeTracking() { std::map, EdgeType>::iterator it; - for (it = m_edgePointsCandidates.begin(); it != m_edgePointsCandidates.end(); it++) { + for (it = m_edgePointsCandidates.begin(); it != m_edgePointsCandidates.end(); ++it) { if (it->second == STRONG_EDGE) { m_edgeMap[it->first.first][it->first.second] = 255; } @@ -456,17 +459,17 @@ vpCannyEdgeDetection::recursiveSearchForStrongEdge(const std::pair(coordinates.first); idRow = std::max(idRow, 0); // Avoid getting negative pixel ID - int idCol = dc + (int)coordinates.second; + int idCol = dc + static_cast(coordinates.second); idCol = std::max(idCol, 0); // Avoid getting negative pixel ID // Checking if we are still looking for an edge in the limit of the image - if ((idRow < 0 || idRow >= nbRows) - || (idCol < 0 || idCol >= nbCols) - || (dr == 0 && dc == 0) + if (((idRow < 0) || (idRow >= nbRows)) + || ((idCol < 0) || (idCol >= nbCols)) + || ((dr == 0) && (dc == 0)) ) { continue; } diff --git a/modules/core/src/image/vpImageCircle.cpp b/modules/core/src/image/vpImageCircle.cpp index ffeb35c263..c679512051 100644 --- a/modules/core/src/image/vpImageCircle.cpp +++ b/modules/core/src/image/vpImageCircle.cpp @@ -69,15 +69,15 @@ vpImageCircle::vpImageCircle(const cv::Vec3f &vec) void computeIntersectionsLeftBorderOnly(const float &u_c, const float &umin_roi, const float &radius, float &delta_theta) { - // umin_roi = u_c + r cos(theta) - // theta = acos((umin_roi - u_c) / r) + // --comment: umin_roi = u_c + r cos(theta) + // --comment: theta = acos((umin_roi - u_c) / r) float theta1 = std::acos((umin_roi - u_c) / radius); theta1 = vpMath::getAngleBetweenMinPiAndPi(theta1); float theta2 = -1.f * theta1; float theta_min = std::min(theta1, theta2); float theta_max = std::max(theta1, theta2); delta_theta = theta_max - theta_min; - if (u_c < umin_roi && std::abs(delta_theta - 2 * M_PIf) < 2.f * std::numeric_limits::epsilon()) { + if ((u_c < umin_roi) && (std::abs(delta_theta - 2 * M_PIf) < (2.f * std::numeric_limits::epsilon()))) { delta_theta = 0.f; } } @@ -94,15 +94,15 @@ void computeIntersectionsLeftBorderOnly(const float &u_c, const float &umin_roi, void computeIntersectionsRightBorderOnly(const float &u_c, const float &umax_roi, const float &radius, float &delta_theta) { - // u = u_c + r cos(theta) - // theta = acos((u - u_c) / r) + // --comment: u = u_c + r cos(theta) + // --comment: theta = acos((u - u_c) / r) float theta1 = std::acos((umax_roi - u_c) / radius); theta1 = vpMath::getAngleBetweenMinPiAndPi(theta1); float theta2 = -1.f * theta1; float theta_min = std::min(theta1, theta2); float theta_max = std::max(theta1, theta2); - delta_theta = 2.f * M_PIf - (theta_max - theta_min); - if (u_c > umax_roi && std::abs(delta_theta - 2 * M_PIf) < 2.f * std::numeric_limits::epsilon()) { + delta_theta = (2.f * M_PIf) - (theta_max - theta_min); + if ((u_c > umax_roi) && (std::abs(delta_theta - (2 * M_PIf)) < (2.f * std::numeric_limits::epsilon()))) { delta_theta = 0.f; } } @@ -133,18 +133,18 @@ void computeIntersectionsTopBorderOnly(const float &v_c, const float &vmin_roi, } float theta_min = std::min(theta1, theta2); float theta_max = std::max(theta1, theta2); - if (std::abs(theta_max - theta_min) * radius < 1.f) { + if ((std::abs(theta_max - theta_min) * radius) < 1.f) { // Between the maximum and minimum theta there is less than 1 pixel of difference // It meens that the full circle is visible delta_theta = 2.f * M_PIf; } else if (theta1 > 0.f) { - delta_theta = 2.f * M_PIf - (theta_max - theta_min); + delta_theta = (2.f * M_PIf) - (theta_max - theta_min); } else { delta_theta = theta_max - theta_min; } - if (v_c < vmin_roi && std::abs(delta_theta - 2 * M_PIf) < 2.f * std::numeric_limits::epsilon()) { + if ((v_c < vmin_roi) && (std::abs(delta_theta - (2 * M_PIf)) < (2.f * std::numeric_limits::epsilon()))) { delta_theta = 0.f; } } @@ -175,7 +175,7 @@ void computeIntersectionsBottomBorderOnly(const float &v_c, const float &vmax_ro } float theta_min = std::min(theta1, theta2); float theta_max = std::max(theta1, theta2); - if (std::abs(theta_max - theta_min) * radius < 1.f) { + if ((std::abs(theta_max - theta_min) * radius) < 1.f) { // Between the maximum and minimum theta there is less than 1 pixel of difference // It meens that the full circle is visible delta_theta = 2.f * M_PIf; @@ -184,9 +184,9 @@ void computeIntersectionsBottomBorderOnly(const float &v_c, const float &vmax_ro delta_theta = theta_max - theta_min; } else { - delta_theta = 2.f * M_PIf - (theta_max - theta_min); + delta_theta = (2.f * M_PIf) - (theta_max - theta_min); } - if (v_c > vmax_roi && std::abs(delta_theta - 2 * M_PIf) < 2.f * std::numeric_limits::epsilon()) { + if ((v_c > vmax_roi) && (std::abs(delta_theta - (2 * M_PIf)) < (2.f * std::numeric_limits::epsilon()))) { delta_theta = 0.f; } } @@ -223,8 +223,8 @@ void computePerpendicularAxesIntersections(const float &u_c, const float &v_c, c theta_u_cross_2 = -M_PIf - theta_u_cross; } // Computing the corresponding u-coordinates at which the u-axis is crossed - float u_ucross = u_c + radius * std::cos(theta_u_cross); - float u_ucross2 = u_c + radius * std::cos(theta_u_cross_2); + float u_ucross = u_c + (radius * std::cos(theta_u_cross)); + float u_ucross2 = u_c + (radius * std::cos(theta_u_cross_2)); // Sorting the outputs such as theta_X_cross_min.second < theta_X_cross_max.second if (u_ucross < u_ucross2) { theta_u_cross_min.first = theta_u_cross; @@ -247,8 +247,8 @@ void computePerpendicularAxesIntersections(const float &u_c, const float &v_c, c float theta_v_cross_2 = -theta_v_cross; // Computing the corresponding v-coordinates at which the v-axis is crossed // v = v_c - radius sin(theta) because the v-axis is oriented towards the bottom - float v_vcross = v_c - radius * std::sin(theta_v_cross); - float v_vcross2 = v_c - radius * std::sin(theta_v_cross_2); + float v_vcross = v_c - (radius * std::sin(theta_v_cross)); + float v_vcross2 = v_c - (radius * std::sin(theta_v_cross_2)); // Sorting the outputs such as theta_X_cross_min.second < theta_X_cross_max.second if (v_vcross < v_vcross2) { theta_v_cross_min.first = theta_v_cross; @@ -291,23 +291,23 @@ void computeIntersectionsTopLeft(const float &u_c, const float &v_c, const float float u_umax = crossing_theta_u_max.second; float v_vmin = crossing_theta_v_min.second; float v_vmax = crossing_theta_v_max.second; - if (u_umin < umin_roi && u_umax >= umin_roi && v_vmin < vmin_roi && v_vmax >= vmin_roi) { + if ((u_umin < umin_roi) && (u_umax >= umin_roi) && (v_vmin < vmin_roi) && (v_vmax >= vmin_roi)) { // The circle crosses only once each axis //Case crossing once delta_theta = theta_u_max - theta_v_max; } - else if (u_umin >= umin_roi && u_umax >= umin_roi && v_vmin >= vmin_roi && v_vmax >= vmin_roi) { + else if ((u_umin >= umin_roi) && (u_umax >= umin_roi) && (v_vmin >= vmin_roi) && (v_vmax >= vmin_roi)) { // The circle crosses twice each axis //Case crossing twice delta_theta = (theta_v_min - theta_u_min) + (theta_u_max - theta_v_max); } - else if (u_umin < umin_roi && u_umax < umin_roi && v_vmin >= vmin_roi && v_vmax >= vmin_roi) { + else if ((u_umin < umin_roi) && (u_umax < umin_roi) && (v_vmin >= vmin_roi) && (v_vmax >= vmin_roi)) { // The circle crosses the u-axis outside the roi // so it is equivalent to the case of crossing only the left border //Case left only computeIntersectionsLeftBorderOnly(u_c, umin_roi, radius, delta_theta); } - else if (u_umin >= umin_roi && u_umax >= umin_roi && v_vmin <= vmin_roi && v_vmax <= vmin_roi) { + else if ((u_umin >= umin_roi) && (u_umax >= umin_roi) && (v_vmin <= vmin_roi) && (v_vmax <= vmin_roi)) { // The circle crosses the v-axis outside the roi // so it is equivalent to the case of crossing only the top border //Case top only @@ -340,7 +340,7 @@ void computeIntersectionsTopRight(const float &u_c, const float &v_c, const floa float u_umax = crossing_theta_u_max.second; float v_vmin = crossing_theta_v_min.second; float v_vmax = crossing_theta_v_max.second; - if (u_umin <= umax_roi && v_vmin < vmin_roi && u_umax >= umax_roi && v_vmax >= vmin_roi) { + if ((u_umin <= umax_roi) && (v_vmin < vmin_roi) && (u_umax >= umax_roi) && (v_vmax >= vmin_roi)) { // The circle crosses only once each axis and the center is below the top border //Case crossing once delta_theta = theta_v_max - theta_u_min; @@ -349,18 +349,18 @@ void computeIntersectionsTopRight(const float &u_c, const float &v_c, const floa delta_theta += 2.f * M_PIf; } } - else if (u_umin <= umax_roi && v_vmin >= vmin_roi && u_umax <= umax_roi && v_vmax >= vmin_roi) { + else if ((u_umin <= umax_roi) && (v_vmin >= vmin_roi) && (u_umax <= umax_roi) && (v_vmax >= vmin_roi)) { // The circle crosses twice each axis //Case crossing twice - delta_theta = 2 * M_PIf - ((theta_u_min - theta_u_max) + (theta_v_min - theta_v_max)); + delta_theta = (2 * M_PIf) - ((theta_u_min - theta_u_max) + (theta_v_min - theta_v_max)); } - else if (u_umin >= umax_roi && v_vmin >= vmin_roi && u_umax >= umax_roi && v_vmax >= vmin_roi) { + else if ((u_umin >= umax_roi) && (v_vmin >= vmin_roi) && (u_umax >= umax_roi) && (v_vmax >= vmin_roi)) { // The circle crosses the u-axis outside the roi // so it is equivalent to the case of crossing only the right border //Case crossing right only computeIntersectionsRightBorderOnly(u_c, umax_roi, radius, delta_theta); } - else if (u_umin <= umax_roi && v_vmin <= vmin_roi && u_umax <= umax_roi && v_vmax <= vmin_roi) { + else if ((u_umin <= umax_roi) && (v_vmin <= vmin_roi) && (u_umax <= umax_roi) && (v_vmax <= vmin_roi)) { // The circle crosses the v-axis outside the roi // so it is equivalent to the case of crossing only the top border //Case crossing top only @@ -395,23 +395,23 @@ void computeIntersectionsBottomLeft(const float &u_c, const float &v_c, const fl float u_umax = crossing_theta_u_max.second; float v_vmin = crossing_theta_v_min.second; float v_vmax = crossing_theta_v_max.second; - if (u_umin < umin_roi && u_umax >= umin_roi && v_vmin <= vmax_roi && v_vmax > vmax_roi) { + if ((u_umin < umin_roi) && (u_umax >= umin_roi) && (v_vmin <= vmax_roi) && (v_vmax > vmax_roi)) { // The circle crosses only once each axis //Case crossing once delta_theta = theta_v_min - theta_u_max; } - else if (u_umin >= umin_roi && u_umax >= umin_roi && v_vmin <= vmax_roi && v_vmax <= vmax_roi) { + else if ((u_umin >= umin_roi) && (u_umax >= umin_roi) && (v_vmin <= vmax_roi) && (v_vmax <= vmax_roi)) { // The circle crosses twice each axis //Case crossing twice delta_theta = (theta_v_min - theta_u_max) + (theta_u_min - theta_v_max); } - else if (u_umin < umin_roi && u_umax < umin_roi && v_vmin <= vmax_roi && v_vmax <= vmax_roi) { + else if ((u_umin < umin_roi) && (u_umax < umin_roi) && (v_vmin <= vmax_roi) && (v_vmax <= vmax_roi)) { // The circle crosses the u-axis outside the roi // so it is equivalent to the case of crossing only the left border //Case left only computeIntersectionsLeftBorderOnly(u_c, umin_roi, radius, delta_theta); } - else if (u_umin >= umin_roi && u_umax >= umin_roi && v_vmin >= vmax_roi && v_vmax >= vmax_roi) { + else if ((u_umin >= umin_roi) && (u_umax >= umin_roi) && (v_vmin >= vmax_roi) && (v_vmax >= vmax_roi)) { // The circle crosses the v-axis outside the roi // so it is equivalent to the case of crossing only the bottom border //Case bottom only @@ -446,7 +446,7 @@ void computeIntersectionsBottomRight(const float &u_c, const float &v_c, const f float u_umax = crossing_theta_u_max.second; float v_vmin = crossing_theta_v_min.second; float v_vmax = crossing_theta_v_max.second; - if (u_umin <= umax_roi && u_umax > umax_roi && v_vmin <= vmax_roi && v_vmax > vmax_roi) { + if ((u_umin <= umax_roi) && (u_umax > umax_roi) && (v_vmin <= vmax_roi) && (v_vmax > vmax_roi)) { // The circle crosses only once each axis //Case crossing once delta_theta = theta_u_min - theta_v_min; @@ -455,18 +455,18 @@ void computeIntersectionsBottomRight(const float &u_c, const float &v_c, const f delta_theta += 2.f * M_PIf; } } - else if (u_umin <= umax_roi && u_umax <= umax_roi && v_vmin <= vmax_roi && v_vmax <= vmax_roi) { + else if ((u_umin <= umax_roi) && (u_umax <= umax_roi) && (v_vmin <= vmax_roi) && (v_vmax <= vmax_roi)) { // The circle crosses twice each axis //Case crossing twice - delta_theta = 2.f * M_PIf - ((theta_v_min - theta_v_max) + (theta_u_max - theta_u_min)); + delta_theta = (2.f * M_PIf) - ((theta_v_min - theta_v_max) + (theta_u_max - theta_u_min)); } - else if (u_umin > umax_roi && u_umax > umax_roi && v_vmin <= vmax_roi && v_vmax <= vmax_roi) { + else if ((u_umin > umax_roi) && (u_umax > umax_roi) && (v_vmin <= vmax_roi) && (v_vmax <= vmax_roi)) { // The circle crosses the u-axis outside the roi // so it is equivalent to the case of crossing only the right border //Case left only computeIntersectionsRightBorderOnly(u_c, umax_roi, radius, delta_theta); } - else if (u_umin <= umax_roi && u_umax <= umax_roi && v_vmin > vmax_roi && v_vmax > vmax_roi) { + else if ((u_umin <= umax_roi) && (u_umax <= umax_roi) && (v_vmin > vmax_roi) && (v_vmax > vmax_roi)) { // The circle crosses the v-axis outside the roi // so it is equivalent to the case of crossing only the bottom border //Case bottom only @@ -513,23 +513,23 @@ void computeIntersectionsTopLeftBottom(const float &u_c, const float &v_c, const 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; - if (u_umin_top >= umin_roi && u_umin_bottom >= umin_roi && v_vmin >= vmin_roi && v_vmax <= vmax_roi) { + if ((u_umin_top >= umin_roi) && (u_umin_bottom >= umin_roi) && (v_vmin >= vmin_roi) && (v_vmax <= vmax_roi)) { // case intersection top + left + bottom twice delta_theta = (theta_v_min - theta_u_min_top) + (theta_u_max_top - theta_u_max_bottom) + (theta_u_min_bottom - theta_v_max); } - else if (u_umin_top <= umin_roi && v_vmin <= vmin_roi && u_umin_bottom <= umin_roi && v_vmax >= vmax_roi) { + else if ((u_umin_top <= umin_roi) && (v_vmin <= vmin_roi) && (u_umin_bottom <= umin_roi) && (v_vmax >= vmax_roi)) { // case intersection top and bottom delta_theta = (theta_u_max_top - theta_u_max_bottom); } - else if (u_umax_top <= umin_roi && u_umax_bottom <= umin_roi && v_vmin >= vmin_roi && v_vmax <= vmax_roi) { + else if ((u_umax_top <= umin_roi) && (u_umax_bottom <= umin_roi) && (v_vmin >= vmin_roi) && (v_vmax <= vmax_roi)) { // case left only computeIntersectionsLeftBorderOnly(u_c, umin_roi, radius, delta_theta); } - else if (u_umax_bottom > umin_roi && v_vmin >= vmin_roi) { + else if ((u_umax_bottom > umin_roi) && (v_vmin >= vmin_roi)) { // case bottom/left corner computeIntersectionsBottomLeft(u_c, v_c, umin_roi, vmax_roi, radius, delta_theta); } - else if (u_umax_top > umin_roi && v_vmax <= vmax_roi) { + else if ((u_umax_top > umin_roi) && (v_vmax <= vmax_roi)) { // case top/left corner computeIntersectionsTopLeft(u_c, v_c, umin_roi, vmin_roi, radius, delta_theta); } @@ -574,23 +574,23 @@ void computeIntersectionsTopRightBottom(const float &u_c, const float &v_c, cons 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; - if (u_umax_top <= umax_roi && u_umax_bottom <= umax_roi && v_vmin >= vmin_roi && v_vmax <= vmax_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_PIf - ((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 ((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)) { // case intersection top and bottom delta_theta = (theta_u_max_top - theta_u_max_bottom); } - else if (u_umin_top >= umax_roi && u_umin_bottom >= umax_roi && v_vmin >= vmin_roi && v_vmax <= vmax_roi) { + else if ((u_umin_top >= umax_roi) && (u_umin_bottom >= umax_roi) && (v_vmin >= vmin_roi) && (v_vmax <= vmax_roi)) { // case right only computeIntersectionsRightBorderOnly(u_c, umax_roi, radius, delta_theta); } - else if (u_umin_bottom <= umax_roi && v_vmin >= vmin_roi) { + else if ((u_umin_bottom <= umax_roi) && (v_vmin >= vmin_roi)) { // case bottom/right corner computeIntersectionsBottomRight(u_c, v_c, vmax_roi, umax_roi, radius, delta_theta); } - else if (u_umin_top <= umax_roi && v_vmax <= vmax_roi) { + else if ((u_umin_top <= umax_roi) && (v_vmax <= vmax_roi)) { // case top/right corner computeIntersectionsTopRight(u_c, v_c, vmin_roi, umax_roi, radius, delta_theta); } @@ -624,8 +624,8 @@ void computeIntersectionsTopBottomOnly(const float &u_c, const float &v_c, const } // Computing the corresponding u-coordinates at which the u-axis is crossed - float u_ucross_top = u_c + radius * std::cos(theta_u_cross_top); - float u_ucross_top_2 = u_c + radius * std::cos(theta_u_cross_top_2); + float u_ucross_top = u_c + (radius * std::cos(theta_u_cross_top)); + float u_ucross_top_2 = u_c + (radius * std::cos(theta_u_cross_top_2)); // Sorting the outputs such as u(theta_u_cross_top_min) < u(theta_u_cross_top_max) float theta_u_cross_top_min = 0.f, theta_u_cross_top_max = 0.f; if (u_ucross_top < u_ucross_top_2) { @@ -651,8 +651,8 @@ void computeIntersectionsTopBottomOnly(const float &u_c, const float &v_c, const } // Computing the corresponding u-coordinates at which the u-axis is crossed - float u_ucross_bottom = u_c + radius * std::cos(theta_u_cross_bottom); - float u_ucross_bottom_2 = u_c + radius * std::cos(theta_u_cross_bottom_2); + float u_ucross_bottom = u_c + (radius * std::cos(theta_u_cross_bottom)); + float u_ucross_bottom_2 = u_c + (radius * std::cos(theta_u_cross_bottom_2)); // Sorting the outputs such as u(theta_u_cross_bottom_min) < u(theta_u_cross_bottom_max) float theta_u_cross_bottom_min = 0.f, theta_u_cross_bottom_max = 0.f; @@ -667,7 +667,7 @@ void computeIntersectionsTopBottomOnly(const float &u_c, const float &v_c, const // Computing the the length of the angular interval of the circle when it intersects // only with the top and bottom borders of the Region of Interest (RoI) - delta_theta = 2.f * M_PIf - ((theta_u_cross_top_min - theta_u_cross_top_max) + (theta_u_cross_bottom_max - theta_u_cross_bottom_min)); + delta_theta = (2.f * M_PIf) - ((theta_u_cross_top_min - theta_u_cross_top_max) + (theta_u_cross_bottom_max - theta_u_cross_bottom_min)); } /*! @@ -712,23 +712,23 @@ void computeIntersectionsLeftRightTop(const float &u_c, const float &v_c, const float v_vmin_right = crossing_theta_v_min.second; float v_vmax_right = crossing_theta_v_max.second; - if (u_umin >= umin_roi && u_umax <= umax_roi && v_vmin_left >= vmin_roi && v_vmin_right >= vmin_roi) { + if ((u_umin >= umin_roi) && (u_umax <= umax_roi) && (v_vmin_left >= vmin_roi) && (v_vmin_right >= vmin_roi)) { // case intersection left + right + top twice delta_theta = (theta_v_min_left - theta_u_min) + (theta_u_max - theta_v_min_right) + (theta_v_max_right - theta_v_max_left); } - else if (u_umin <= umin_roi && u_umax >= umax_roi && v_vmax_left >= vmin_roi && v_vmax_right >= vmin_roi) { + else if ((u_umin <= umin_roi) && (u_umax >= umax_roi) && (v_vmax_left >= vmin_roi) && (v_vmax_right >= vmin_roi)) { // case intersection left + right delta_theta = (theta_v_max_right - theta_v_max_left); } - else if (v_vmax_left <= vmin_roi && v_vmax_right <= vmin_roi && u_umin >= umin_roi && u_umax <= umax_roi) { + else if ((v_vmax_left <= vmin_roi) && (v_vmax_right <= vmin_roi) && (u_umin >= umin_roi) && (u_umax <= umax_roi)) { // case top only computeIntersectionsTopBorderOnly(v_c, vmin_roi, radius, delta_theta); } - else if (u_umax >= umin_roi && v_vmax_left >= vmin_roi) { + else if ((u_umax >= umin_roi) && (v_vmax_left >= vmin_roi)) { // case top/left corner computeIntersectionsTopLeft(u_c, v_c, umin_roi, vmin_roi, radius, delta_theta); } - else if (u_umin <= umax_roi && v_vmax_right >= vmin_roi) { + else if ((u_umin <= umax_roi) && (v_vmax_right >= vmin_roi)) { // case top/right corner computeIntersectionsTopRight(u_c, v_c, vmin_roi, umax_roi, radius, delta_theta); } @@ -764,7 +764,7 @@ void computeIntersectionsLeftRightBottom(const float &u_c, const float &v_c, con float theta_v_min_left = crossing_theta_v_min.first; float theta_v_max_left = crossing_theta_v_max.first; float v_vmin_left = crossing_theta_v_min.second; - // float v_vmax_left = crossing_theta_v_max.second; + // --comment: float v_vmax_left = crossing_theta_v_max.second // Computing the intersections with the bottom and right axes float crossing_v_right = umax_roi; // We cross the v-axis of the right of the RoI at the maximum u-coordinate of the RoI @@ -774,25 +774,25 @@ void computeIntersectionsLeftRightBottom(const float &u_c, const float &v_c, con float theta_v_min_right = crossing_theta_v_min.first; float theta_v_max_right = crossing_theta_v_max.first; float v_vmin_right = crossing_theta_v_min.second; - // float v_vmax_right = crossing_theta_v_max.second; + // --comment: float v_vmax_right = crossing_theta_v_max.second - if (u_umin >= umin_roi && u_umax <= umax_roi && v_vmin_left <= vmax_roi && v_vmin_right <= vmax_roi) { + if ((u_umin >= umin_roi) && (u_umax <= umax_roi) && (v_vmin_left <= vmax_roi) && (v_vmin_right <= vmax_roi)) { // case intersection left + right + bottom twice delta_theta = (theta_v_min_left - theta_v_min_right) + (theta_v_max_right - theta_u_max) + (theta_u_min - theta_v_max_left); } - else if (u_umin <= umin_roi && u_umax >= umax_roi && v_vmin_left <= vmax_roi && v_vmin_right <= vmax_roi) { + else if ((u_umin <= umin_roi) && (u_umax >= umax_roi) && (v_vmin_left <= vmax_roi) && (v_vmin_right <= vmax_roi)) { // case intersection left + right delta_theta = (theta_v_min_left - theta_v_min_right); } - else if (v_vmin_left >= vmax_roi && v_vmin_right >= vmax_roi && u_umin >= umin_roi && u_umax <= umax_roi) { + else if ((v_vmin_left >= vmax_roi) && (v_vmin_right >= vmax_roi) && (u_umin >= umin_roi) && (u_umax <= umax_roi)) { // case bottom only computeIntersectionsBottomBorderOnly(v_c, vmax_roi, radius, delta_theta); } - else if (u_umax >= umin_roi && v_vmin_right >= vmax_roi) { + else if ((u_umax >= umin_roi) && (v_vmin_right >= vmax_roi)) { // case bottom/left corner computeIntersectionsBottomLeft(u_c, v_c, umin_roi, vmax_roi, radius, delta_theta); } - else if (u_umin <= umax_roi && v_vmin_right <= vmax_roi) { + else if ((u_umin <= umax_roi) && (v_vmin_right <= vmax_roi)) { // case bottom/right corner computeIntersectionsBottomRight(u_c, v_c, vmax_roi, umax_roi, radius, delta_theta); } @@ -821,8 +821,8 @@ void computeIntersectionsLeftRightOnly(const float &u_c, const float &v_c, const float theta_v_cross_left_2 = -theta_v_cross_left; // Computing the corresponding v-coordinates at which the v-axis is crossed - float v_vcross_left = v_c - radius * std::sin(theta_v_cross_left); - float v_vcross_left_2 = v_c - radius * std::sin(theta_v_cross_left_2); + float v_vcross_left = v_c - (radius * std::sin(theta_v_cross_left)); + float v_vcross_left_2 = v_c - (radius * std::sin(theta_v_cross_left_2)); // Sorting the outputs such as v(theta_v_cross_left_min) < v(theta_v_cross_left_max) float theta_v_cross_left_min = 0.f, theta_v_cross_left_max = 0.f; if (v_vcross_left < v_vcross_left_2) { @@ -843,8 +843,8 @@ void computeIntersectionsLeftRightOnly(const float &u_c, const float &v_c, const float theta_v_cross_right_2 = -theta_v_cross_right; // Computing the corresponding v-coordinates at which the v-axis is crossed - float v_vcross_right = v_c - radius * std::sin(theta_v_cross_right); - float v_vcross_right_2 = v_c - radius * std::sin(theta_v_cross_right_2); + float v_vcross_right = v_c - (radius * std::sin(theta_v_cross_right)); + float v_vcross_right_2 = v_c - (radius * std::sin(theta_v_cross_right_2)); // Sorting the outputs such as v(theta_v_cross_right_min) < v(theta_v_cross_right_max) float theta_v_cross_right_min = 0.f, theta_v_cross_right_max = 0.f; @@ -924,67 +924,67 @@ float vpImageCircle::computeAngularCoverageInRoI(const vpRect &roi, const float bool touchRightBorder = (u_c + radius) >= umax_roi; bool touchTopBorder = (v_c - radius) <= vmin_roi; bool touchBottomBorder = (v_c + radius) >= vmax_roi; - bool isHorizontallyOK = (!touchLeftBorder && !touchRightBorder); - bool isVerticallyOK = (!touchTopBorder && !touchBottomBorder); + bool isHorizontallyOK = ((!touchLeftBorder) && (!touchRightBorder)); + bool isVerticallyOK = ((!touchTopBorder) && (!touchBottomBorder)); if (isHorizontallyOK && isVerticallyOK && roi.isInside(m_center)) { // Easy case // The circle has its center in the image and its radius is not too great // to make it fully contained in the RoI delta_theta = 2.f * M_PIf; } - else if (touchBottomBorder && !touchLeftBorder && !touchRightBorder && !touchTopBorder) { + else if (touchBottomBorder && (!touchLeftBorder) && (!touchRightBorder) && (!touchTopBorder)) { // Touches/intersects only the bottom border of the RoI computeIntersectionsBottomBorderOnly(v_c, vmax_roi, radius, delta_theta); } - else if (!touchBottomBorder && touchLeftBorder && !touchRightBorder && !touchTopBorder) { + else if ((!touchBottomBorder) && touchLeftBorder && (!touchRightBorder) && (!touchTopBorder)) { // Touches/intersects only the left border of the RoI computeIntersectionsLeftBorderOnly(u_c, umin_roi, radius, delta_theta); } - else if (!touchBottomBorder && !touchLeftBorder && touchRightBorder && !touchTopBorder) { + else if ((!touchBottomBorder) && (!touchLeftBorder) && touchRightBorder && (!touchTopBorder)) { // Touches/intersects only the right border of the RoI computeIntersectionsRightBorderOnly(u_c, umax_roi, radius, delta_theta); } - else if (!touchBottomBorder && !touchLeftBorder && !touchRightBorder && touchTopBorder) { + else if ((!touchBottomBorder) && (!touchLeftBorder) && (!touchRightBorder) && touchTopBorder) { // Touches/intersects only the top border of the RoI computeIntersectionsTopBorderOnly(v_c, vmin_roi, radius, delta_theta); } - else if (touchBottomBorder && touchLeftBorder && !touchRightBorder && !touchTopBorder) { + else if (touchBottomBorder && touchLeftBorder && (!touchRightBorder) && (!touchTopBorder)) { // Touches/intersects the bottom and left borders of the RoI computeIntersectionsBottomLeft(u_c, v_c, umin_roi, vmax_roi, radius, delta_theta); } - else if (touchBottomBorder && !touchLeftBorder && touchRightBorder && !touchTopBorder) { + else if (touchBottomBorder && (!touchLeftBorder) && touchRightBorder && (!touchTopBorder)) { // Touches/intersects the bottom and right borders of the RoI computeIntersectionsBottomRight(u_c, v_c, vmax_roi, umax_roi, radius, delta_theta); } - else if (!touchBottomBorder && touchLeftBorder && !touchRightBorder && touchTopBorder) { + else if ((!touchBottomBorder) && touchLeftBorder && (!touchRightBorder) && touchTopBorder) { // Touches/intersects the top and left borders of the RoI computeIntersectionsTopLeft(u_c, v_c, umin_roi, vmin_roi, radius, delta_theta); } - else if (!touchBottomBorder && !touchLeftBorder && touchRightBorder && touchTopBorder) { + else if ((!touchBottomBorder) && (!touchLeftBorder) && touchRightBorder && touchTopBorder) { // Touches/intersects the top and right borders of the RoI computeIntersectionsTopRight(u_c, v_c, vmin_roi, umax_roi, radius, delta_theta); } - else if (touchBottomBorder && touchTopBorder && touchLeftBorder && !touchRightBorder) { + else if (touchBottomBorder && touchTopBorder && touchLeftBorder && (!touchRightBorder)) { // Touches/intersects the top, left and bottom borders of the RoI computeIntersectionsTopLeftBottom(u_c, v_c, umin_roi, vmin_roi, vmax_roi, radius, delta_theta); } - else if (touchBottomBorder && touchTopBorder && !touchLeftBorder && touchRightBorder) { + else if (touchBottomBorder && touchTopBorder && (!touchLeftBorder) && touchRightBorder) { // Touches/intersects the top, right and bottom borders of the RoI computeIntersectionsTopRightBottom(u_c, v_c, umax_roi, vmin_roi, vmax_roi, radius, delta_theta); } - else if (touchBottomBorder && touchTopBorder && !touchLeftBorder && !touchRightBorder) { + else if (touchBottomBorder && touchTopBorder && (!touchLeftBorder) && (!touchRightBorder)) { // Touches/intersects the top and bottom borders of the RoI computeIntersectionsTopBottomOnly(u_c, v_c, vmin_roi, vmax_roi, radius, delta_theta); } - else if (!touchBottomBorder && touchTopBorder && touchLeftBorder && touchRightBorder) { + else if ((!touchBottomBorder) && touchTopBorder && touchLeftBorder && touchRightBorder) { // Touches/intersects the top, left and right borders of the RoI computeIntersectionsLeftRightTop(u_c, v_c, umin_roi, umax_roi, vmin_roi, radius, delta_theta); } - else if (touchBottomBorder && !touchTopBorder && touchLeftBorder && touchRightBorder) { + else if (touchBottomBorder && (!touchTopBorder) && touchLeftBorder && touchRightBorder) { // Touches/intersects the bottom, left and right borders of the RoI computeIntersectionsLeftRightBottom(u_c, v_c, umin_roi, umax_roi, vmax_roi, radius, delta_theta); } - else if (touchLeftBorder && touchRightBorder && !touchTopBorder && !touchBottomBorder) { + else if (touchLeftBorder && touchRightBorder && (!touchTopBorder) && (!touchBottomBorder)) { // Touches/intersects the bottom, left and right borders of the RoI computeIntersectionsLeftRightOnly(u_c, v_c, umin_roi, umax_roi, radius, delta_theta); } @@ -1001,9 +1001,9 @@ float vpImageCircle::computeAngularCoverageInRoI(const vpRect &roi, const float throw(vpException(vpException::fatalError, "This case should never happen. Please contact Inria to make fix the problem")); } - if (delta_theta < 0 || delta_theta > 2.f * M_PIf) { // Needed since M_PIf is used + if ((delta_theta < 0) || (delta_theta > (2.f * M_PIf))) { // Needed since M_PIf is used float rest = vpMath::modulo(delta_theta, 2.f * M_PIf); - if (rest < roundingTolerance && (delta_theta < -M_PIf || delta_theta > M_PIf)) { + if ((rest < roundingTolerance) && ((delta_theta < -M_PIf) || (delta_theta > M_PIf))) { // If the angle is a negative multiple of 2.f * M_PIf we consider it to be 2.f * M_PIf delta_theta = 2.f * M_PIf; } @@ -1096,23 +1096,23 @@ unsigned int vpImageCircle::computePixelsInMask(const vpImage &mask) const float dthetaSinNeg = -1.f / (r_float * sin_theta); float dthetaPos = 0.f; if ((sin_theta < 0.f) && (cos_theta > 0.f)) { - // dTheta <= -1/r sin(theta) && dTheta <= 1/r cos(theta) + // --comment: dTheta <= -1/r sin(theta) && dTheta <= 1/r cos(theta) dthetaPos = std::min(dthetaCosPos, dthetaSinNeg); } else if ((sin_theta > 0.f) && (cos_theta < 0.f)) { - // dTheta <= 1/r sin(theta) && dTheta <= -1/r cos(theta) + // --comment: dTheta <= 1/r sin(theta) && dTheta <= -1/r cos(theta) dthetaPos = std::min(dthetaCosNeg, dthetaSinPos); } else if ((sin_theta < 0.f) && (cos_theta < 0.f)) { - // dTheta <= -1/r sin(theta) && dTheta <= -1/r cos(theta) + // --comment: dTheta <= -1/r sin(theta) && dTheta <= -1/r cos(theta) dthetaPos = std::min(dthetaCosNeg, dthetaSinNeg); } else if ((sin_theta > 0.f) && (cos_theta > 0.f)) { - // dTheta <= 1/r sin(theta) && dTheta <= 1/r cos(theta) + // --comment: dTheta <= 1/r sin(theta) && dTheta <= 1/r cos(theta) dthetaPos = std::min(dthetaCosPos, dthetaSinPos); } else if (vpMath::equal(sin_theta, 0.f) && (!vpMath::equal(cos_theta, 0.f))) { - // dTheta = -1 / r cos(theta) || dTheta = 1 / r cos(theta) + // --comment: dTheta = -1 / r cos(theta) || dTheta = 1 / r cos(theta) if (cos_theta > 0.f) { dthetaPos = dthetaCosNeg; } @@ -1121,7 +1121,7 @@ unsigned int vpImageCircle::computePixelsInMask(const vpImage &mask) const } } else if ((!vpMath::equal(sin_theta, 0.f)) && vpMath::equal(cos_theta, 0.f)) { - // dTheta = -1 / r sin(theta) || dTheta = 1 / r sin(theta) + // --comment: dTheta = -1 / r sin(theta) || dTheta = 1 / r sin(theta) if (sin_theta > 0.f) { dthetaPos = dthetaSinNeg; } @@ -1152,12 +1152,12 @@ vpRect vpImageCircle::getBBox() const float vpImageCircle::get_n20() const { - return m_radius * m_radius / 4; + return (m_radius * m_radius) / 4; } float vpImageCircle::get_n02() const { - return m_radius * m_radius / 4; + return (m_radius * m_radius) / 4; } float vpImageCircle::get_n11() const diff --git a/modules/core/src/image/vpImageConvert.cpp b/modules/core/src/image/vpImageConvert.cpp index 6913994fdb..8328ff8bb3 100644 --- a/modules/core/src/image/vpImageConvert.cpp +++ b/modules/core/src/image/vpImageConvert.cpp @@ -1095,9 +1095,9 @@ void vpImageConvert::RGBToRGBa(unsigned char *rgb, unsigned char *rgba, unsigned for (i = 0; i < height; ++i) { unsigned char *line = src; for (j = 0; j < width; ++j) { - *rgba++ = *(line++); - *rgba++ = *(line++); - *rgba++ = *(line++); + *rgba++ = *(++line); + *rgba++ = *(++line); + *rgba++ = *(++line); *rgba++ = vpRGBa::alpha_default; } // go to the next line @@ -1130,9 +1130,9 @@ void vpImageConvert::RGBaToRGB(unsigned char *rgba, unsigned char *rgb, unsigned unsigned char *pt_output = rgb; while (pt_input != pt_end) { - *(pt_output++) = *(pt_input++); // R - *(pt_output++) = *(pt_input++); // G - *(pt_output++) = *(pt_input++); // B + *(++pt_output) = *(++pt_input); // R + *(++pt_output) = *(++pt_input); // G + *(++pt_output) = *(++pt_input); // B pt_input++; } #endif @@ -1192,9 +1192,9 @@ void vpImageConvert::RGBToGrey(unsigned char *rgb, unsigned char *grey, unsigned for (i = 0; i < height; ++i) { unsigned char *line = src; for (j = 0; j < width; ++j) { - r = *(line++); - g = *(line++); - b = *(line++); + r = *(++line); + g = *(++line); + b = *(++line); *grey++ = static_cast((0.2126 * r) + (0.7152 * g) + (0.0722 * b)); } diff --git a/modules/core/src/image/vpImageFilter.cpp b/modules/core/src/image/vpImageFilter.cpp index a19508fa56..23de0acd44 100644 --- a/modules/core/src/image/vpImageFilter.cpp +++ b/modules/core/src/image/vpImageFilter.cpp @@ -275,7 +275,7 @@ void vpImageFilter::sepFilter(const vpImage &I, vpImage & for (unsigned int j = half_size; j < (widthI - half_size); ++j) { double conv = 0.0; for (unsigned int a = 0; a < size; ++a) { - conv += kernelH[a] * static_cast(I[i][j + half_size - a]); + conv += kernelH[a] * static_cast(I[i][(j + half_size) - a]); } I_filter[i][j] = conv; @@ -286,7 +286,7 @@ void vpImageFilter::sepFilter(const vpImage &I, vpImage & for (unsigned int j = 0; j < widthI; ++j) { double conv = 0.0; for (unsigned int a = 0; a < sizeV; ++a) { - conv += kernelV[a] * I_filter[i + half_size - a][j]; + conv += kernelV[a] * I_filter[(i + half_size) - a][j]; } If[i][j] = conv; @@ -319,10 +319,10 @@ void vpImageFilter::filterX(const vpImage &I, vpImage &dIx, cons { const unsigned int heightI = I.getHeight(), widthI = I.getWidth(); const unsigned int stop1J = (size - 1) / 2; - const unsigned int stop2J = widthI - (size - 1) / 2; + const unsigned int stop2J = widthI - ((size - 1) / 2); resizeAndInitializeIfNeeded(p_mask, heightI, widthI, dIx); - for (unsigned int i = 0; i < heightI; i++) { + for (unsigned int i = 0; i < heightI; ++i) { for (unsigned int j = 0; j < stop1J; ++j) { // We have to compute the value for each pixel if we don't have a mask or for // pixels for which the mask is true otherwise @@ -381,7 +381,7 @@ void vpImageFilter::filterY(const vpImage &I, vpImage &dIy, cons { const unsigned int heightI = I.getHeight(), widthI = I.getWidth(); const unsigned int stop1I = (size - 1) / 2; - const unsigned int stop2I = heightI - (size - 1) / 2; + const unsigned int stop2I = heightI - ((size - 1) / 2); resizeAndInitializeIfNeeded(p_mask, heightI, widthI, dIy); for (unsigned int i = 0; i < stop1I; ++i) { @@ -580,7 +580,7 @@ void vpImageFilter::getGaussXPyramidal(const vpImage &I, vpImage< for (unsigned int j = 1; j < (w - 1); ++j) { GI[i][j] = vpImageFilter::filterGaussXPyramidal(I, i, 2 * j); } - GI[i][w - 1] = I[i][2 * w - 1]; + GI[i][w - 1] = I[i][(2 * w) - 1]; } } @@ -595,7 +595,7 @@ void vpImageFilter::getGaussYPyramidal(const vpImage &I, vpImage< for (unsigned int i = 1; i < (h - 1); ++i) { GI[i][j] = vpImageFilter::filterGaussYPyramidal(I, 2 * i, j); } - GI[h - 1][j] = I[2 * h - 1][j]; + GI[h - 1][j] = I[(2 * h) - 1][j]; } } diff --git a/modules/core/src/image/vpImagePoint.cpp b/modules/core/src/image/vpImagePoint.cpp index 5669d9ef0c..dfe495bbea 100644 --- a/modules/core/src/image/vpImagePoint.cpp +++ b/modules/core/src/image/vpImagePoint.cpp @@ -48,8 +48,8 @@ */ bool vpImagePoint::inRectangle(const vpRect &rect) const { - return (this->i <= rect.getBottom() && this->i >= rect.getTop() && this->j <= rect.getRight() && - this->j >= rect.getLeft()); + return ((this->i <= rect.getBottom()) && (this->i >= rect.getTop()) && (this->j <= rect.getRight()) && + (this->j >= rect.getLeft())); } /*! @@ -71,7 +71,7 @@ int main() ip[1].set_ij(300, 400); vpImagePoint cog(0,0); - for(unsigned int i=0; i::epsilon()) && - (std::fabs(j1 - j2) <= std::fabs(vpMath::maximum(j1, j2)) * std::numeric_limits::epsilon())); + return ((std::fabs(i1 - i2) <= (std::fabs(vpMath::maximum(i1, i2)) * std::numeric_limits::epsilon())) && + (std::fabs(j1 - j2) <= (std::fabs(vpMath::maximum(j1, j2)) * std::numeric_limits::epsilon()))); } /*! @@ -145,15 +144,14 @@ VISP_EXPORT bool operator==(const vpImagePoint &ip1, const vpImagePoint &ip2) */ VISP_EXPORT bool operator!=(const vpImagePoint &ip1, const vpImagePoint &ip2) { - // return ( ( ip1.get_i() != ip2.get_i() ) || ( ip1.get_j() != ip2.get_j() ) - // ); + // --comment: return ( ( ip1.get_i() != ip2.get_i() ) || ( ip1.get_j() != ip2.get_j() )); double i1 = ip1.get_i(); double j1 = ip1.get_j(); double i2 = ip2.get_i(); double j2 = ip2.get_j(); - return ((std::fabs(i1 - i2) > std::fabs(vpMath::maximum(i1, i2)) * std::numeric_limits::epsilon()) || - (std::fabs(j1 - j2) > std::fabs(vpMath::maximum(j1, j2)) * std::numeric_limits::epsilon())); + return ((std::fabs(i1 - i2) > (std::fabs(vpMath::maximum(i1, i2)) * std::numeric_limits::epsilon())) || + (std::fabs(j1 - j2) > (std::fabs(vpMath::maximum(j1, j2)) * std::numeric_limits::epsilon()))); } /*! diff --git a/modules/core/src/image/vpImageTools.cpp b/modules/core/src/image/vpImageTools.cpp index 1853b89e7a..90a0ba6c78 100644 --- a/modules/core/src/image/vpImageTools.cpp +++ b/modules/core/src/image/vpImageTools.cpp @@ -109,8 +109,8 @@ void vpImageTools::changeLUT(vpImage &I, unsigned char A, unsigne double factor = (double)(B_star - A_star) / (double)(B - A); - for (unsigned int i = 0; i < I.getHeight(); i++) - for (unsigned int j = 0; j < I.getWidth(); j++) { + for (unsigned int i = 0; i < I.getHeight(); ++i) + for (unsigned int j = 0; j < I.getWidth(); ++j) { v = I[i][j]; if (v <= A) @@ -220,7 +220,7 @@ void vpImageTools::imageDifferenceAbsolute(const vpImage &I1, con } unsigned int n = I1.getHeight() * I1.getWidth(); - for (unsigned int b = 0; b < n; b++) { + for (unsigned int b = 0; b < n; ++b) { int diff = I1.bitmap[b] - I2.bitmap[b]; Idiff.bitmap[b] = static_cast(vpMath::abs(diff)); } @@ -244,7 +244,7 @@ void vpImageTools::imageDifferenceAbsolute(const vpImage &I1, const vpIm } unsigned int n = I1.getHeight() * I1.getWidth(); - for (unsigned int b = 0; b < n; b++) { + for (unsigned int b = 0; b < n; ++b) { Idiff.bitmap[b] = vpMath::abs(I1.bitmap[b] - I2.bitmap[b]); } } @@ -273,7 +273,7 @@ void vpImageTools::imageDifferenceAbsolute(const vpImage &I1, const vpIm } unsigned int n = I1.getHeight() * I1.getWidth(); - for (unsigned int b = 0; b < n; b++) { + for (unsigned int b = 0; b < n; ++b) { int diffR = I1.bitmap[b].R - I2.bitmap[b].R; int diffG = I1.bitmap[b].G - I2.bitmap[b].G; int diffB = I1.bitmap[b].B - I2.bitmap[b].B; @@ -411,8 +411,8 @@ void vpImageTools::initUndistortMap(const vpCameraParameters &cam, unsigned int if (!is_KannalaBrandt && std::fabs(static_cast(kud)) <= std::numeric_limits::epsilon()) { // There is no need to undistort the image (Perpective projection) - for (unsigned int i = 0; i < height; i++) { - for (unsigned int j = 0; j < width; j++) { + for (unsigned int i = 0; i < height; ++i) { + for (unsigned int j = 0; j < width; ++j) { mapU[i][j] = static_cast(j); mapV[i][j] = static_cast(i); mapDu[i][j] = 0; @@ -441,7 +441,7 @@ void vpImageTools::initUndistortMap(const vpCameraParameters &cam, unsigned int kud_py2 = kud * invpy * invpy; } - for (unsigned int v = 0; v < height; v++) { + for (unsigned int v = 0; v < height; ++v) { deltav = v - v0; if (!is_KannalaBrandt) @@ -449,7 +449,7 @@ void vpImageTools::initUndistortMap(const vpCameraParameters &cam, unsigned int else deltav_py = deltav * invpy; - for (unsigned int u = 0; u < width; u++) { + for (unsigned int u = 0; u < width; ++u) { // computation of u,v : corresponding pixel coordinates in I. deltau = u - u0; if (!is_KannalaBrandt) { @@ -511,8 +511,8 @@ void vpImageTools::integralImage(const vpImage &I, vpImage &I1, const vpIm #if defined(VISP_HAVE_SIMDLIB) SimdNormalizedCorrelation(I1.bitmap, a, I2.bitmap, b, I1.getSize(), a2, b2, ab, useOptimized); #else - for (unsigned int cpt = 0; cpt < I1.getSize(); cpt++) { + for (unsigned int cpt = 0; cpt < I1.getSize(); ++cpt) { ab += (I1.bitmap[cpt] - a) * (I2.bitmap[cpt] - b); a2 += vpMath::sqr(I1.bitmap[cpt] - a); b2 += vpMath::sqr(I2.bitmap[cpt] - b); @@ -723,7 +723,7 @@ void vpImageTools::templateMatching(const vpImage &I, const vpIma // zero-mean template image const double sum2 = (II_tpl[height_tpl][width_tpl] + II_tpl[0][0] - II_tpl[0][width_tpl] - II_tpl[height_tpl][0]); const double mean2 = sum2 / I_tpl.getSize(); - for (unsigned int cpt = 0; cpt < I_tpl_double.getSize(); cpt++) { + for (unsigned int cpt = 0; cpt < I_tpl_double.getSize(); ++cpt) { I_tpl_double.bitmap[cpt] -= mean2; } @@ -744,7 +744,7 @@ void vpImageTools::templateMatching(const vpImage &I, const vpIma #if defined(_OPENMP) // only to disable warning: ignoring #pragma omp parallel [-Wunknown-pragmas] #pragma omp parallel for schedule(dynamic) #endif - for (int cpt = 0; cpt < end; cpt++) { + for (int cpt = 0; cpt < end; ++cpt) { for (unsigned int j = 0; j < I.getWidth() - width_tpl; j += step_u) { I_score[vec_step_v[cpt]][j] = normalizedCorrelation(I_double, I_tpl_double, II, IIsq, II_tpl, IIsq_tpl, vec_step_v[cpt], j); @@ -801,8 +801,8 @@ double vpImageTools::normalizedCorrelation(const vpImage &I1, const vpIm #if defined(VISP_HAVE_SIMDLIB) SimdNormalizedCorrelation2(I1.bitmap, I1.getWidth(), I2.bitmap, I2.getWidth(), I2.getHeight(), i0, j0, ab); #else - for (unsigned int i = 0; i < I2.getHeight(); i++) { - for (unsigned int j = 0; j < I2.getWidth(); j++) { + for (unsigned int i = 0; i < I2.getHeight(); ++i) { + for (unsigned int j = 0; j < I2.getWidth(); ++j) { ab += (I1[i0 + i][j0 + j]) * I2[i][j]; } } @@ -841,9 +841,9 @@ void vpImageTools::remap(const vpImage &I, const vpArray2D & #if defined(_OPENMP) // only to disable warning: ignoring #pragma omp parallel [-Wunknown-pragmas] #pragma omp parallel for schedule(dynamic) #endif - for (int i_ = 0; i_ < static_cast(I.getHeight()); i_++) { + for (int i_ = 0; i_ < static_cast(I.getHeight()); ++i_) { const unsigned int i = static_cast(i_); - for (unsigned int j = 0; j < I.getWidth(); j++) { + for (unsigned int j = 0; j < I.getWidth(); ++j) { int u_round = mapU[i][j]; int v_round = mapV[i][j]; @@ -885,13 +885,13 @@ void vpImageTools::remap(const vpImage &I, const vpArray2D &mapU, c #if defined(_OPENMP) // only to disable warning: ignoring #pragma omp parallel [-Wunknown-pragmas] #pragma omp parallel for schedule(dynamic) #endif - for (int i = 0; i < static_cast(I.getHeight()); i++) { + for (int i = 0; i < static_cast(I.getHeight()); ++i) { #if defined(VISP_HAVE_SIMDLIB) SimdRemap(reinterpret_cast(I.bitmap), 4, I.getWidth(), I.getHeight(), i * I.getWidth(), mapU.data, mapV.data, mapDu.data, mapDv.data, reinterpret_cast(Iundist.bitmap)); #else const unsigned int i_ = static_cast(i); - for (unsigned int j = 0; j < I.getWidth(); j++) { + for (unsigned int j = 0; j < I.getWidth(); ++j) { int u_round = mapU[i_][j]; int v_round = mapV[i_][j]; diff --git a/modules/core/src/image/vpRGBa.cpp b/modules/core/src/image/vpRGBa.cpp index 69147272c4..3e8ec6a227 100644 --- a/modules/core/src/image/vpRGBa.cpp +++ b/modules/core/src/image/vpRGBa.cpp @@ -99,10 +99,10 @@ vpRGBa &vpRGBa::operator=(const vpColVector &v) vpERROR_TRACE("Bad vector dimension "); throw(vpException(vpException::dimensionError, "Bad vector dimension ")); } - R = (unsigned char)v[0]; - G = (unsigned char)v[1]; - B = (unsigned char)v[2]; - A = (unsigned char)v[3]; + R = static_cast(v[0]); + G = static_cast(v[1]); + B = static_cast(v[2]); + A = static_cast(v[3]); return *this; } @@ -113,14 +113,14 @@ vpRGBa &vpRGBa::operator=(const vpColVector &v) */ bool vpRGBa::operator==(const vpRGBa &v) const { - return R == v.R && G == v.G && B == v.B && A == v.A; + return (R == v.R) && (G == v.G) && (B == v.B) && (A == v.A); } /*! Compare two color pixels. \return true if the images are different, false if they are the same. */ -bool vpRGBa::operator!=(const vpRGBa &v) const { return (R != v.R || G != v.G || B != v.B || A != v.A); } +bool vpRGBa::operator!=(const vpRGBa &v) const { return ((R != v.R) || (G != v.G) || (B != v.B) || (A != v.A)); } /*! subtraction operator : "this" - v. @@ -130,10 +130,10 @@ bool vpRGBa::operator!=(const vpRGBa &v) const { return (R != v.R || G != v.G || vpColVector vpRGBa::operator-(const vpRGBa &v) const { vpColVector n(4); // new color - n[0] = (double)R - (double)v.R; - n[1] = (double)G - (double)v.G; - n[2] = (double)B - (double)v.B; - n[3] = (double)A - (double)v.A; + n[0] = static_cast(R) - static_cast(v.R); + n[1] = static_cast(G) - static_cast(v.G); + n[2] = static_cast(B) - static_cast(v.B); + n[3] = static_cast(A) - static_cast(v.A); return n; } @@ -215,16 +215,16 @@ vpColVector vpRGBa::operator*(const double &v) const bool vpRGBa::operator<(const vpRGBa &v) const { - double gray1 = 0.2126 * R + 0.7152 * G + 0.0722 * B; - double gray2 = 0.2126 * v.R + 0.7152 * v.G + 0.0722 * v.B; + double gray1 = (0.2126 * R) + (0.7152 * G) + (0.0722 * B); + double gray2 = (0.2126 * v.R) + (0.7152 * v.G) + (0.0722 * v.B); return (gray1 < gray2); } bool vpRGBa::operator>(const vpRGBa &v) const { - double gray1 = 0.2126 * R + 0.7152 * G + 0.0722 * B; - double gray2 = 0.2126 * v.R + 0.7152 * v.G + 0.0722 * v.B; + double gray1 = (0.2126 * R) + (0.7152 * G) + (0.0722 * B); + double gray2 = (0.2126 * v.R) + (0.7152 * v.G) + (0.0722 * v.B); return (gray1 > gray2); } @@ -255,6 +255,6 @@ int main() */ VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpRGBa &rgba) { - os << "(" << (int)rgba.R << "," << (int)rgba.G << "," << (int)rgba.B << "," << (int)rgba.A << ")"; + os << "(" << static_cast(rgba.R) << "," << static_cast(rgba.G) << "," << static_cast(rgba.B) << "," << static_cast(rgba.A) << ")"; return os; } diff --git a/modules/core/src/image/vpRGBf.cpp b/modules/core/src/image/vpRGBf.cpp index 5663ced711..2979de6f81 100644 --- a/modules/core/src/image/vpRGBf.cpp +++ b/modules/core/src/image/vpRGBf.cpp @@ -96,9 +96,9 @@ vpRGBf &vpRGBf::operator=(const vpColVector &v) vpERROR_TRACE("Bad vector dimension"); throw(vpException(vpException::dimensionError, "Bad vector dimension")); } - R = (float)v[0]; - G = (float)v[1]; - B = (float)v[2]; + R = static_cast(v[0]); + G = static_cast(v[1]); + B = static_cast(v[2]); return *this; } @@ -109,12 +109,15 @@ vpRGBf &vpRGBf::operator=(const vpColVector &v) */ bool vpRGBf::operator==(const vpRGBf &v) const { - if (std::fabs(R - v.R) > std::numeric_limits::epsilon()) + if (std::fabs(R - v.R) > std::numeric_limits::epsilon()) { return false; - if (std::fabs(G - v.G) > std::numeric_limits::epsilon()) + } + if (std::fabs(G - v.G) > std::numeric_limits::epsilon()) { return false; - if (std::fabs(B - v.B) > std::numeric_limits::epsilon()) + } + if (std::fabs(B - v.B) > std::numeric_limits::epsilon()) { return false; + } return true; } @@ -125,9 +128,9 @@ bool vpRGBf::operator==(const vpRGBf &v) const */ bool vpRGBf::operator!=(const vpRGBf &v) const { - return (std::fabs(R - v.R) > std::numeric_limits::epsilon() || - std::fabs(G - v.G) > std::numeric_limits::epsilon() || - std::fabs(B - v.B) > std::numeric_limits::epsilon()); + return ((std::fabs(R - v.R) > std::numeric_limits::epsilon()) || + (std::fabs(G - v.G) > std::numeric_limits::epsilon()) || + (std::fabs(B - v.B) > std::numeric_limits::epsilon())); } /*! @@ -138,9 +141,9 @@ bool vpRGBf::operator!=(const vpRGBf &v) const vpColVector vpRGBf::operator-(const vpRGBf &v) const { vpColVector n(3); // new color - n[0] = (double)R - (double)v.R; - n[1] = (double)G - (double)v.G; - n[2] = (double)B - (double)v.B; + n[0] = static_cast(R) - static_cast(v.R); + n[1] = static_cast(G) - static_cast(v.G); + n[2] = static_cast(B) - static_cast(v.B); return n; } @@ -217,16 +220,16 @@ vpColVector vpRGBf::operator*(double v) const bool vpRGBf::operator<(const vpRGBf &v) const { - double gray1 = 0.2126 * R + 0.7152 * G + 0.0722 * B; - double gray2 = 0.2126 * v.R + 0.7152 * v.G + 0.0722 * v.B; + double gray1 = (0.2126 * R) + (0.7152 * G) + (0.0722 * B); + double gray2 = (0.2126 * v.R) + (0.7152 * v.G) + (0.0722 * v.B); return (gray1 < gray2); } bool vpRGBf::operator>(const vpRGBf &v) const { - double gray1 = 0.2126 * R + 0.7152 * G + 0.0722 * B; - double gray2 = 0.2126 * v.R + 0.7152 * v.G + 0.0722 * v.B; + double gray1 = (0.2126 * R) + (0.7152 * G) + (0.0722 * B); + double gray2 = (0.2126 * v.R) + (0.7152 * v.G) + (0.0722 * v.B); return (gray1 > gray2); } diff --git a/modules/core/src/math/matrix/vpColVector.cpp b/modules/core/src/math/matrix/vpColVector.cpp index fc79ee2686..7e4a7c7c3a 100644 --- a/modules/core/src/math/matrix/vpColVector.cpp +++ b/modules/core/src/math/matrix/vpColVector.cpp @@ -65,8 +65,9 @@ vpColVector vpColVector::operator+(const vpColVector &v) const } vpColVector r(rowNum); - for (unsigned int i = 0; i < rowNum; i++) + for (unsigned int i = 0; i < rowNum; ++i) { r[i] = (*this)[i] + v[i]; + } return r; } @@ -78,8 +79,9 @@ vpTranslationVector vpColVector::operator+(const vpTranslationVector &t) const } vpTranslationVector s; - for (unsigned int i = 0; i < 3; i++) + for (unsigned int i = 0; i < 3; ++i) { s[i] = (*this)[i] + t[i]; + } return s; } @@ -91,8 +93,9 @@ vpColVector &vpColVector::operator+=(vpColVector v) v.getRows())); } - for (unsigned int i = 0; i < rowNum; i++) + for (unsigned int i = 0; i < rowNum; ++i) { (*this)[i] += v[i]; + } return (*this); } @@ -103,8 +106,9 @@ vpColVector &vpColVector::operator-=(vpColVector v) getRows(), v.getRows())); } - for (unsigned int i = 0; i < rowNum; i++) + for (unsigned int i = 0; i < rowNum; ++i) { (*this)[i] -= v[i]; + } return (*this); } @@ -118,16 +122,18 @@ double vpColVector::operator*(const vpColVector &v) const } double r = 0; - for (unsigned int i = 0; i < rowNum; i++) + for (unsigned int i = 0; i < rowNum; ++i) { r += (*this)[i] * v[i]; + } return r; } vpMatrix vpColVector::operator*(const vpRowVector &v) const { vpMatrix M(rowNum, v.getCols()); - for (unsigned int i = 0; i < rowNum; i++) { - for (unsigned int j = 0; j < v.getCols(); j++) { + unsigned int v_cols = v.getCols(); + for (unsigned int i = 0; i < rowNum; ++i) { + for (unsigned int j = 0; j < v_cols; ++j) { M[i][j] = (*this)[i] * v[j]; } } @@ -144,8 +150,9 @@ vpColVector vpColVector::operator-(const vpColVector &m) const } vpColVector v(rowNum); - for (unsigned int i = 0; i < rowNum; i++) + for (unsigned int i = 0; i < rowNum; ++i) { v[i] = (*this)[i] - m[i]; + } return v; } @@ -158,41 +165,50 @@ void vpColVector::init(const vpColVector &v, unsigned int r, unsigned int nrows) { unsigned int rnrows = r + nrows; - if (rnrows > v.getRows()) + if (rnrows > v.getRows()) { throw(vpException(vpException::dimensionError, "Bad row dimension (%d > %d) used to initialize vpColVector", rnrows, v.getRows())); + } resize(nrows, false); if (this->rowPtrs == nullptr) { // Fix coverity scan: explicit null dereferenced return; // Nothing to do } - for (unsigned int i = r; i < rnrows; i++) { + for (unsigned int i = r; i < rnrows; ++i) { (*this)[i - r] = v[i]; } } vpColVector::vpColVector(const vpRotationVector &v) : vpArray2D(v.size(), 1) { - for (unsigned int i = 0; i < v.size(); i++) + unsigned int v_size = v.size(); + for (unsigned int i = 0; i < v_size; ++i) { (*this)[i] = v[i]; + } } vpColVector::vpColVector(const vpPoseVector &p) : vpArray2D(p.size(), 1) { - for (unsigned int i = 0; i < p.size(); i++) + unsigned int p_size = p.size(); + for (unsigned int i = 0; i < p_size; ++i) { (*this)[i] = p[i]; + } } vpColVector::vpColVector(const vpTranslationVector &v) : vpArray2D(v.size(), 1) { - for (unsigned int i = 0; i < v.size(); i++) + unsigned int v_size = v.size(); + for (unsigned int i = 0; i < v_size; ++i) { (*this)[i] = v[i]; + } } vpColVector::vpColVector(const vpMatrix &M, unsigned int j) : vpArray2D(M.getRows(), 1) { - for (unsigned int i = 0; i < M.getCols(); i++) + unsigned int m_cols = M.getCols(); + for (unsigned int i = 0; i < m_cols; ++i) { (*this)[i] = M[i][j]; + } } vpColVector::vpColVector(const vpMatrix &M) : vpArray2D(M.getRows(), 1) @@ -202,20 +218,26 @@ vpColVector::vpColVector(const vpMatrix &M) : vpArray2D(M.getRows(), 1) M.getRows(), M.getRows(), M.getCols())); } - for (unsigned int i = 0; i < M.getRows(); i++) + unsigned int m_rows = M.getRows(); + for (unsigned int i = 0; i < m_rows; ++i) { (*this)[i] = M[i][0]; + } } -vpColVector::vpColVector(const std::vector &v) : vpArray2D((unsigned int)v.size(), 1) +vpColVector::vpColVector(const std::vector &v) : vpArray2D(static_cast(v.size()), 1) { - for (unsigned int i = 0; i < v.size(); i++) + unsigned int v_size = v.size(); + for (unsigned int i = 0; i < v_size; ++i) { (*this)[i] = v[i]; + } } -vpColVector::vpColVector(const std::vector &v) : vpArray2D((unsigned int)v.size(), 1) +vpColVector::vpColVector(const std::vector &v) : vpArray2D(static_cast(v.size()), 1) { - for (unsigned int i = 0; i < v.size(); i++) - (*this)[i] = (double)(v[i]); + unsigned int v_size = v.size(); + for (unsigned int i = 0; i < v_size; ++i) { + (*this)[i] = static_cast(v[i]); + } } #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) @@ -243,8 +265,10 @@ vpColVector vpColVector::operator-() const double *vd = A.data; double *d = data; - for (unsigned int i = 0; i < rowNum; i++) + for (unsigned int i = 0; i < rowNum; ++i) { + // move the d++ increment/decrement into a dedicated expression-statement *(vd++) = -(*d++); + } return A; } @@ -256,22 +280,26 @@ vpColVector vpColVector::operator*(double x) const double *vd = v.data; double *d = data; - for (unsigned int i = 0; i < rowNum; i++) + for (unsigned int i = 0; i < rowNum; ++i) { + // move the d++ increment/decrement into a dedicated expression-statement *(vd++) = (*d++) * x; + } return v; } vpColVector &vpColVector::operator*=(double x) { - for (unsigned int i = 0; i < rowNum; i++) + for (unsigned int i = 0; i < rowNum; ++i) { (*this)[i] *= x; + } return (*this); } vpColVector &vpColVector::operator/=(double x) { - for (unsigned int i = 0; i < rowNum; i++) + for (unsigned int i = 0; i < rowNum; ++i) { (*this)[i] /= x; + } return (*this); } @@ -282,8 +310,10 @@ vpColVector vpColVector::operator/(double x) const double *vd = v.data; double *d = data; - for (unsigned int i = 0; i < rowNum; i++) + for (unsigned int i = 0; i < rowNum; ++i) { + // move the d++ increment/decrement into a dedicated expression-statement *(vd++) = (*d++) / x; + } return v; } @@ -302,17 +332,21 @@ vpColVector &vpColVector::operator=(const vpMatrix &M) vpColVector &vpColVector::operator=(const std::vector &v) { - resize((unsigned int)v.size(), false); - for (unsigned int i = 0; i < v.size(); i++) + unsigned int v_size = v.size(); + resize(v_size, false); + for (unsigned int i = 0; i < v_size; ++i) { (*this)[i] = v[i]; + } return *this; } vpColVector &vpColVector::operator=(const std::vector &v) { - resize((unsigned int)v.size(), false); - for (unsigned int i = 0; i < v.size(); i++) - (*this)[i] = (float)v[i]; + unsigned int v_size = v.size(); + resize(v_size, false); + for (unsigned int i = 0; i < v_size; ++i) { + (*this)[i] = static_cast(v[i]); + } return *this; } @@ -368,8 +402,8 @@ vpColVector &vpColVector::operator<<(const vpColVector &v) vpColVector &vpColVector::operator<<(double *x) { - for (unsigned int i = 0; i < rowNum; i++) { - for (unsigned int j = 0; j < colNum; j++) { + for (unsigned int i = 0; i < rowNum; ++i) { + for (unsigned int j = 0; j < colNum; ++j) { rowPtrs[i][j] = *x++; } } @@ -394,8 +428,9 @@ vpColVector &vpColVector::operator=(double x) { double *d = data; - for (unsigned int i = 0; i < rowNum; i++) + for (unsigned int i = 0; i < rowNum; ++i) { *(d++) = x; + } return *this; } @@ -403,8 +438,10 @@ std::vector vpColVector::toStdVector() const { std::vector v(this->size()); - for (unsigned int i = 0; i < this->size(); i++) + unsigned int v_this_size = this->size(); + for (unsigned int i = 0; i < v_this_size; ++i) { v[i] = data[i]; + } return v; } @@ -441,12 +478,14 @@ vpColVector &vpColVector::operator=(const std::initializer_list &list) bool vpColVector::operator==(const vpColVector &v) const { - if (rowNum != v.rowNum || colNum != v.colNum /* should not happen */) + if ((rowNum != v.rowNum) || (colNum != v.colNum) /* should not happen */) { return false; + } - for (unsigned int i = 0; i < rowNum; i++) { - if (!vpMath::equal(data[i], v.data[i], std::numeric_limits::epsilon())) + for (unsigned int i = 0; i < rowNum; ++i) { + if (!vpMath::equal(data[i], v.data[i], std::numeric_limits::epsilon())) { return false; + } } return true; @@ -454,9 +493,10 @@ bool vpColVector::operator==(const vpColVector &v) const bool vpColVector::operator==(double v) const { - for (unsigned int i = 0; i < rowNum; i++) { - if (!vpMath::equal(data[i], v, std::numeric_limits::epsilon())) + for (unsigned int i = 0; i < rowNum; ++i) { + if (!vpMath::equal(data[i], v, std::numeric_limits::epsilon())) { return false; + } } return true; @@ -503,8 +543,11 @@ double vpColVector::dotProd(const vpColVector &a, const vpColVector &b) double *bd = b.data; double c = 0; - for (unsigned int i = 0; i < a.getRows(); i++) + unsigned int a_rows_nbr = a.getRows(); + for (unsigned int i = 0; i < a_rows_nbr; ++i) { + // Move the ad++ and bd++ increment/decrement into a dedicated expression-statement c += *(ad++) * *(bd++); + } return c; } @@ -519,12 +562,11 @@ vpColVector &vpColVector::normalize(vpColVector &x) const vpColVector &vpColVector::normalize() { - double sum_square = sumSquare(); - // if (sum != 0.0) - if (std::fabs(sum_square) > std::numeric_limits::epsilon()) + if (std::fabs(sum_square) > std::numeric_limits::epsilon()) { *this /= sqrt(sum_square); + } // If sum = 0, we have a nul vector. So we return just. return *this; @@ -541,8 +583,8 @@ vpColVector vpColVector::invSort(const vpColVector &v) unsigned int i = 0; while (nb_permutation != 0) { nb_permutation = 0; - for (unsigned int j = v.getRows() - 1; j >= i + 1; j--) { - if ((tab[j] > tab[j - 1])) { + for (unsigned int j = v.getRows() - 1; j >= (i + 1); j--) { + if (tab[j] > tab[j - 1]) { double tmp = tab[j]; tab[j] = tab[j - 1]; tab[j - 1] = tmp; @@ -566,8 +608,8 @@ vpColVector vpColVector::sort(const vpColVector &v) unsigned int i = 0; while (nb_permutation != 0) { nb_permutation = 0; - for (unsigned int j = v.getRows() - 1; j >= i + 1; j--) { - if ((tab[j] < tab[j - 1])) { + for (unsigned int j = v.getRows() - 1; j >= (i + 1); j--) { + if (tab[j] < tab[j - 1]) { double tmp = tab[j]; tab[j] = tab[j - 1]; tab[j - 1] = tmp; @@ -600,7 +642,7 @@ void vpColVector::stack(const vpColVector &A, const vpColVector &B, vpColVector unsigned int nrA = A.getRows(); unsigned int nrB = B.getRows(); - if (nrA == 0 && nrB == 0) { + if ((nrA == 0) && (nrB == 0)) { C.resize(0); return; } @@ -618,16 +660,18 @@ void vpColVector::stack(const vpColVector &A, const vpColVector &B, vpColVector // General case C.resize(nrA + nrB, false); - for (unsigned int i = 0; i < nrA; i++) + for (unsigned int i = 0; i < nrA; ++i) { C[i] = A[i]; + } - for (unsigned int i = 0; i < nrB; i++) + for (unsigned int i = 0; i < nrB; ++i) { C[nrA + i] = B[i]; + } } double vpColVector::mean(const vpColVector &v) { - if (v.data == nullptr || v.size() == 0) { + if ((v.data == nullptr) || (v.size() == 0)) { throw(vpException(vpException::dimensionError, "Cannot compute column vector mean: vector empty")); } @@ -639,7 +683,7 @@ double vpColVector::mean(const vpColVector &v) double vpColVector::median(const vpColVector &v) { - if (v.data == nullptr || v.size() == 0) { + if ((v.data == nullptr) || (v.size() == 0)) { throw(vpException(vpException::dimensionError, "Cannot compute column vector median: vector empty")); } @@ -650,7 +694,7 @@ double vpColVector::median(const vpColVector &v) double vpColVector::stdev(const vpColVector &v, bool useBesselCorrection) { - if (v.data == nullptr || v.size() == 0) { + if ((v.data == nullptr) || (v.size() == 0)) { throw(vpException(vpException::dimensionError, "Cannot compute column vector stdev: vector empty")); } #if defined(VISP_HAVE_SIMDLIB) @@ -695,7 +739,7 @@ vpMatrix vpColVector::skew(const vpColVector &v) vpColVector vpColVector::crossProd(const vpColVector &a, const vpColVector &b) { - if (a.getRows() != 3 || b.getRows() != 3) { + if ((a.getRows() != 3) || (b.getRows() != 3)) { throw(vpException(vpException::dimensionError, "Cannot compute the cross product between column " "vector with dimension %d and %d", @@ -714,24 +758,28 @@ vpMatrix vpColVector::reshape(unsigned int nrows, unsigned int ncols) void vpColVector::reshape(vpMatrix &M, const unsigned int &nrows, const unsigned int &ncols) { - if (dsize != nrows * ncols) { + if (dsize != (nrows * ncols)) { throw(vpException(vpException::dimensionError, "Cannot reshape (%dx1) column vector in (%dx%d) matrix", rowNum, M.getRows(), M.getCols())); } - if ((M.getRows() != nrows) || (M.getCols() != ncols)) + if ((M.getRows() != nrows) || (M.getCols() != ncols)) { M.resize(nrows, ncols, false, false); + } - for (unsigned int j = 0; j < ncols; j++) - for (unsigned int i = 0; i < nrows; i++) - M[i][j] = data[j * nrows + i]; + for (unsigned int j = 0; j < ncols; ++j) { + for (unsigned int i = 0; i < nrows; ++i) { + M[i][j] = data[(j * nrows) + i]; + } + } } void vpColVector::insert(unsigned int i, const vpColVector &v) { - if (i + v.size() > this->size()) + if ((i + v.size()) > (this->size())) { throw(vpException(vpException::dimensionError, "Unable to insert a column vector")); + } - if (data != nullptr && v.data != nullptr && v.rowNum > 0) { + if ( (data != nullptr) && (v.data != nullptr) && (v.rowNum > 0)) { memcpy(data + i, v.data, sizeof(double) * v.rowNum); } } @@ -748,7 +796,7 @@ int vpColVector::print(std::ostream &s, unsigned int length, char const *intro) std::ostringstream ossFixed; std::ios_base::fmtflags original_flags = oss.flags(); - // ossFixed <(maxAfter, totalLength - maxBefore); - if (maxAfter == 1) + if (maxAfter == 1) { maxAfter = 0; + } // the following line is useful for debugging // std::cerr <(maxBefore)); s << values[i].substr(0, p).c_str(); if (maxAfter > 0) { s.setf(std::ios::left, std::ios::adjustfield); if (p != std::string::npos) { - s.width((std::streamsize)maxAfter); + s.width(static_cast(maxAfter)); s << values[i].substr(p, maxAfter).c_str(); } else { assert(maxAfter > 1); - s.width((std::streamsize)maxAfter); + s.width(static_cast(maxAfter)); s << ".0"; } } @@ -819,7 +869,7 @@ int vpColVector::print(std::ostream &s, unsigned int length, char const *intro) s.flags(original_flags); // restore s to standard state - return (int)(maxBefore + maxAfter); + return static_cast(maxBefore + maxAfter); } double vpColVector::sum() const @@ -857,7 +907,7 @@ double vpColVector::frobeniusNorm() const vpColVector vpColVector::hadamard(const vpColVector &v) const { - if (v.getRows() != rowNum || v.getCols() != colNum) { + if ( (v.getRows() != rowNum) || (v.getCols() != colNum)) { throw(vpException(vpException::dimensionError, "Hadamard product: bad dimensions!")); } @@ -868,7 +918,7 @@ vpColVector vpColVector::hadamard(const vpColVector &v) const #else #endif - for (unsigned int i = 0; i < dsize; i++) { + for (unsigned int i = 0; i < dsize; ++i) { out.data[i] = data[i] * v.data[i]; } return out; @@ -877,7 +927,7 @@ vpColVector vpColVector::hadamard(const vpColVector &v) const double vpColVector::infinityNorm() const { double norm = 0.0; - for (unsigned int i = 0; i < rowNum; i++) { + for (unsigned int i = 0; i < rowNum; ++i) { double x = fabs((*this)[i]); if (x > norm) { norm = x; @@ -890,7 +940,8 @@ std::ostream &vpColVector::cppPrint(std::ostream &os, const std::string &matrixN { os << "vpColVector " << matrixName << " (" << this->getRows() << "); " << std::endl; - for (unsigned int i = 0; i < this->getRows(); ++i) { + unsigned int rows_nbr = this->getRows(); + for (unsigned int i = 0; i < rows_nbr; ++i) { if (!octet) { os << matrixName << "[" << i << "] = " << (*this)[i] << "; " << std::endl; @@ -898,7 +949,7 @@ std::ostream &vpColVector::cppPrint(std::ostream &os, const std::string &matrixN else { for (unsigned int k = 0; k < sizeof(double); ++k) { os << "((unsigned char*)&(" << matrixName << "[" << i << "]) )[" << k << "] = 0x" << std::hex - << (unsigned int)((unsigned char *)&((*this)[i]))[k] << "; " << std::endl; + << static_cast(((unsigned char *)&((*this)[i]))[k]) << "; " << std::endl; } } } @@ -908,7 +959,8 @@ std::ostream &vpColVector::cppPrint(std::ostream &os, const std::string &matrixN std::ostream &vpColVector::csvPrint(std::ostream &os) const { - for (unsigned int i = 0; i < this->getRows(); ++i) { + unsigned int rows_nbr = this->getRows(); + for (unsigned int i = 0; i < rows_nbr; ++i) { os << (*this)[i]; os << std::endl; @@ -918,8 +970,9 @@ std::ostream &vpColVector::csvPrint(std::ostream &os) const std::ostream &vpColVector::maplePrint(std::ostream &os) const { + unsigned int rows_nbr = this->getRows(); os << "([ " << std::endl; - for (unsigned int i = 0; i < this->getRows(); ++i) { + for (unsigned int i = 0; i < rows_nbr; ++i) { os << "["; os << (*this)[i] << ", "; os << "]," << std::endl; @@ -930,10 +983,11 @@ std::ostream &vpColVector::maplePrint(std::ostream &os) const std::ostream &vpColVector::matlabPrint(std::ostream &os) const { + unsigned int rows_nbr = this->getRows(); os << "[ "; - for (unsigned int i = 0; i < this->getRows(); ++i) { + for (unsigned int i = 0; i < rows_nbr; ++i) { os << (*this)[i] << ", "; - if (this->getRows() != i + 1) { + if (this->getRows() != (i + 1)) { os << ";" << std::endl; } else { diff --git a/modules/core/src/math/matrix/vpEigenConversion.cpp b/modules/core/src/math/matrix/vpEigenConversion.cpp index d2e2fc76fb..359cb8999b 100644 --- a/modules/core/src/math/matrix/vpEigenConversion.cpp +++ b/modules/core/src/math/matrix/vpEigenConversion.cpp @@ -48,7 +48,7 @@ void eigen2visp(const Eigen::MatrixXd &src, vpMatrix &dst) void eigen2visp(const Eigen::MatrixXd &src, vpHomogeneousMatrix &dst) { - if (src.rows() != 4 || src.cols() != 4) { + if ((src.rows() != 4) || (src.cols() != 4)) { throw vpException(vpException::dimensionError, "Input Eigen Matrix must be of size (4,4)!"); } @@ -60,25 +60,29 @@ void eigen2visp(const Eigen::VectorXd &src, vpColVector &dst) { dst.resize(static_cast(src.rows())); #if (VP_VERSION_INT(EIGEN_WORLD_VERSION, EIGEN_MAJOR_VERSION, EIGEN_MINOR_VERSION) < 0x030300) - for (Eigen::DenseIndex i = 0; i < src.rows(); i++) { + Eigen::DenseIndex src_rows = src.rows(); + for (Eigen::DenseIndex i = 0; i < src_rows; i++) { #else - for (Eigen::Index i = 0; i < src.rows(); i++) { + Eigen::Index src_rows = src.rows(); + for (Eigen::Index i = 0; i < src_rows; i++) { #endif dst[static_cast(i)] = src(i); } -} + } void eigen2visp(const Eigen::RowVectorXd &src, vpRowVector &dst) { dst.resize(static_cast(src.cols())); #if (VP_VERSION_INT(EIGEN_WORLD_VERSION, EIGEN_MAJOR_VERSION, EIGEN_MINOR_VERSION) < 0x030300) - for (Eigen::DenseIndex i = 0; i < src.cols(); i++) { + Eigen::DenseIndex src_cols = src.cols(); + for (Eigen::DenseIndex i = 0; i < src_cols; ++i) { #else - for (Eigen::Index i = 0; i < src.cols(); i++) { + Eigen::Index src_cols = src.cols(); + for (Eigen::Index i = 0; i < src_cols; i++) { #endif dst[static_cast(i)] = src(i); } -} + } void visp2eigen(const vpColVector &src, Eigen::VectorXd &dst) { dst = Eigen::VectorXd::Map(src.data, src.size()); } diff --git a/modules/core/src/math/matrix/vpMatrix.cpp b/modules/core/src/math/matrix/vpMatrix.cpp index b4d43403b6..3a6939e1b2 100644 --- a/modules/core/src/math/matrix/vpMatrix.cpp +++ b/modules/core/src/math/matrix/vpMatrix.cpp @@ -126,8 +126,9 @@ void compute_pseudo_inverse(const vpMatrix &U, const vpColVector &sv, const vpMa rank_out = 0; - for (unsigned int i = 0; i < sv.size(); i++) { - if (sv[i] > maxsv * svThreshold) { + unsigned int sv_size = sv.size(); + for (unsigned int i = 0; i < sv_size; ++i) { + if (sv[i] > (maxsv * svThreshold)) { rank_out++; } } @@ -137,10 +138,10 @@ void compute_pseudo_inverse(const vpMatrix &U, const vpColVector &sv, const vpMa rank = static_cast(*rank_in); } - for (unsigned int i = 0; i < ncols; i++) { - for (unsigned int j = 0; j < nrows; j++) { - for (unsigned int k = 0; k < rank; k++) { - Ap[i][j] += V[i][k] * U[j][k] / sv[k]; + for (unsigned int i = 0; i < ncols; ++i) { + for (unsigned int j = 0; j < nrows; ++j) { + for (unsigned int k = 0; k < rank; ++k) { + Ap[i][j] += (V[i][k] * U[j][k]) / sv[k]; } } } @@ -149,8 +150,8 @@ void compute_pseudo_inverse(const vpMatrix &U, const vpColVector &sv, const vpMa if (imA) { imA->resize(nrows, rank); - for (unsigned int i = 0; i < nrows; i++) { - for (unsigned int j = 0; j < rank; j++) { + for (unsigned int i = 0; i < nrows; ++i) { + for (unsigned int j = 0; j < rank; ++j) { (*imA)[i][j] = U[i][j]; } } @@ -159,8 +160,8 @@ void compute_pseudo_inverse(const vpMatrix &U, const vpColVector &sv, const vpMa // Compute im(At) if (imAt) { imAt->resize(ncols, rank); - for (unsigned int i = 0; i < ncols; i++) { - for (unsigned int j = 0; j < rank; j++) { + for (unsigned int i = 0; i < ncols; ++i) { + for (unsigned int j = 0; j < rank; ++j) { (*imAt)[i][j] = V[i][j]; } } @@ -170,9 +171,10 @@ void compute_pseudo_inverse(const vpMatrix &U, const vpColVector &sv, const vpMa if (kerAt) { kerAt->resize(ncols - rank, ncols); if (rank != ncols) { - for (unsigned int k = 0; k < (ncols - rank); k++) { + unsigned int v_rows = V.getRows(); + for (unsigned int k = 0; k < (ncols - rank); ++k) { unsigned j = k + rank; - for (unsigned int i = 0; i < V.getRows(); i++) { + for (unsigned int i = 0; i < v_rows; ++i) { (*kerAt)[k][i] = V[i][j]; } } @@ -340,18 +342,20 @@ void vpMatrix::init(const vpMatrix &M, unsigned int r, unsigned int c, unsigned unsigned int rnrows = r + nrows; unsigned int cncols = c + ncols; - if (rnrows > M.getRows()) + if (rnrows > M.getRows()) { throw(vpException(vpException::dimensionError, "Bad row dimension (%d > %d) used to initialize vpMatrix", rnrows, M.getRows())); - if (cncols > M.getCols()) + } + if (cncols > M.getCols()) { throw(vpException(vpException::dimensionError, "Bad column dimension (%d > %d) used to initialize vpMatrix", cncols, M.getCols())); + } resize(nrows, ncols, false, false); if (this->rowPtrs == nullptr) { // Fix coverity scan: explicit null dereferenced return; // Noting to do } - for (unsigned int i = 0; i < nrows; i++) { + for (unsigned int i = 0; i < nrows; ++i) { memcpy((*this)[i], &M[i + r][c], ncols * sizeof(double)); } } @@ -402,16 +406,18 @@ vpMatrix vpMatrix::extract(unsigned int r, unsigned int c, unsigned int nrows, u unsigned int rnrows = r + nrows; unsigned int cncols = c + ncols; - if (rnrows > getRows()) + if (rnrows > getRows()) { throw(vpException(vpException::dimensionError, "Bad row dimension (%d > %d) used to initialize vpMatrix", rnrows, getRows())); - if (cncols > getCols()) + } + if (cncols > getCols()) { throw(vpException(vpException::dimensionError, "Bad column dimension (%d > %d) used to initialize vpMatrix", cncols, getCols())); + } vpMatrix M; M.resize(nrows, ncols, false, false); - for (unsigned int i = 0; i < nrows; i++) { + for (unsigned int i = 0; i < nrows; ++i) { memcpy(M[i], &(*this)[i + r][c], ncols * sizeof(double)); } @@ -441,12 +447,14 @@ void vpMatrix::eye(unsigned int m, unsigned int n) */ void vpMatrix::eye() { - for (unsigned int i = 0; i < rowNum; i++) { - for (unsigned int j = 0; j < colNum; j++) { - if (i == j) + for (unsigned int i = 0; i < rowNum; ++i) { + for (unsigned int j = 0; j < colNum; ++j) { + if (i == j) { (*this)[i][j] = 1.0; - else + } + else { (*this)[i][j] = 0; + } } } } @@ -477,9 +485,9 @@ void vpMatrix::transpose(vpMatrix &At) const { At.resize(colNum, rowNum, false, false); - if (rowNum <= 16 || colNum <= 16) { - for (unsigned int i = 0; i < rowNum; i++) { - for (unsigned int j = 0; j < colNum; j++) { + if ((rowNum <= 16) || (colNum <= 16)) { + for (unsigned int i = 0; i < rowNum; ++i) { + for (unsigned int j = 0; j < colNum; ++j) { At[j][i] = (*this)[i][j]; } } @@ -528,11 +536,12 @@ vpMatrix vpMatrix::AAt() const */ void vpMatrix::AAt(vpMatrix &B) const { - if ((B.rowNum != rowNum) || (B.colNum != rowNum)) + if ((B.rowNum != rowNum) || (B.colNum != rowNum)) { B.resize(rowNum, rowNum, false, false); + } // If available use Lapack only for large matrices - bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size); + bool useLapack = ((rowNum > vpMatrix::m_lapack_min_size) || (colNum > vpMatrix::m_lapack_min_size)); #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)) useLapack = false; #endif @@ -550,19 +559,21 @@ void vpMatrix::AAt(vpMatrix &B) const } else { // compute A*A^T - for (unsigned int i = 0; i < rowNum; i++) { - for (unsigned int j = i; j < rowNum; j++) { + for (unsigned int i = 0; i < rowNum; ++i) { + for (unsigned int j = i; j < rowNum; ++j) { double *pi = rowPtrs[i]; // row i double *pj = rowPtrs[j]; // row j // sum (row i .* row j) double ssum = 0; - for (unsigned int k = 0; k < colNum; k++) + for (unsigned int k = 0; k < colNum; ++k) { ssum += *(pi++) * *(pj++); + } B[i][j] = ssum; // upper triangle - if (i != j) + if (i != j) { B[j][i] = ssum; // lower triangle + } } } } @@ -581,11 +592,12 @@ void vpMatrix::AAt(vpMatrix &B) const */ void vpMatrix::AtA(vpMatrix &B) const { - if ((B.rowNum != colNum) || (B.colNum != colNum)) + if ((B.rowNum != colNum) || (B.colNum != colNum)) { B.resize(colNum, colNum, false, false); + } // If available use Lapack only for large matrices - bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size); + bool useLapack = ((rowNum > vpMatrix::m_lapack_min_size) || (colNum > vpMatrix::m_lapack_min_size)); #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)) useLapack = false; #endif @@ -602,12 +614,12 @@ void vpMatrix::AtA(vpMatrix &B) const #endif } else { - for (unsigned int i = 0; i < colNum; i++) { + for (unsigned int i = 0; i < colNum; ++i) { double *Bi = B[i]; - for (unsigned int j = 0; j < i; j++) { + for (unsigned int j = 0; j < i; ++j) { double *ptr = data; double s = 0; - for (unsigned int k = 0; k < rowNum; k++) { + for (unsigned int k = 0; k < rowNum; ++k) { s += (*(ptr + i)) * (*(ptr + j)); ptr += colNum; } @@ -616,7 +628,7 @@ void vpMatrix::AtA(vpMatrix &B) const } double *ptr = data; double s = 0; - for (unsigned int k = 0; k < rowNum; k++) { + for (unsigned int k = 0; k < rowNum; ++k) { s += (*(ptr + i)) * (*(ptr + i)); ptr += colNum; } @@ -659,7 +671,7 @@ vpMatrix &vpMatrix::operator=(const vpArray2D &A) { resize(A.getRows(), A.getCols(), false, false); - if (data != nullptr && A.data != nullptr && data != A.data) { + if ((data != nullptr) && (A.data != nullptr) && (data != A.data)) { memcpy(data, A.data, dsize * sizeof(double)); } @@ -670,7 +682,7 @@ vpMatrix &vpMatrix::operator=(const vpMatrix &A) { resize(A.getRows(), A.getCols(), false, false); - if (data != nullptr && A.data != nullptr && data != A.data) { + if ((data != nullptr) && (A.data != nullptr) && (data != A.data)) { memcpy(data, A.data, dsize * sizeof(double)); } @@ -775,7 +787,7 @@ vpMatrix &vpMatrix::operator=(const std::initializer_listbegin(), it->end(), rowPtrs[i]); } @@ -786,7 +798,7 @@ vpMatrix &vpMatrix::operator=(const std::initializer_listresize(rows, rows); (*this) = 0; - for (unsigned int i = 0; i < rows; i++) + for (unsigned int i = 0; i < rows; ++i) { (*this)[i][i] = A[i]; + } } /*! @@ -898,8 +911,9 @@ void vpMatrix::diag(const double &val) { (*this) = 0; unsigned int min_ = (rowNum < colNum) ? rowNum : colNum; - for (unsigned int i = 0; i < min_; i++) + for (unsigned int i = 0; i < min_; ++i) { (*this)[i][i] = val; + } } /*! @@ -917,8 +931,9 @@ void vpMatrix::createDiagonalMatrix(const vpColVector &A, vpMatrix &DA) unsigned int rows = A.getRows(); DA.resize(rows, rows, true); - for (unsigned int i = 0; i < rows; i++) + for (unsigned int i = 0; i < rows; ++i) { DA[i][i] = A[i]; + } } /*! @@ -929,17 +944,18 @@ vpTranslationVector vpMatrix::operator*(const vpTranslationVector &tv) const { vpTranslationVector t_out; - if (rowNum != 3 || colNum != 3) { + if ((rowNum != 3) || (colNum != 3)) { throw(vpException(vpException::dimensionError, "Cannot multiply a (%dx%d) matrix by a (%dx%d) translation vector", rowNum, colNum, tv.getRows(), tv.getCols())); } - for (unsigned int j = 0; j < 3; j++) + for (unsigned int j = 0; j < 3; ++j) { t_out[j] = 0; + } - for (unsigned int j = 0; j < 3; j++) { + for (unsigned int j = 0; j < 3; ++j) { double tj = tv[j]; // optimization em 5/12/2006 - for (unsigned int i = 0; i < 3; i++) { + for (unsigned int i = 0; i < 3; ++i) { t_out[i] += rowPtrs[i][j] * tj; } } @@ -972,11 +988,12 @@ void vpMatrix::multMatrixVector(const vpMatrix &A, const vpColVector &v, vpColVe A.getRows(), A.getCols(), v.getRows())); } - if (A.rowNum != w.rowNum) + if (A.rowNum != w.rowNum) { w.resize(A.rowNum, false); + } // If available use Lapack only for large matrices - bool useLapack = (A.rowNum > vpMatrix::m_lapack_min_size || A.colNum > vpMatrix::m_lapack_min_size); + bool useLapack = ((A.rowNum > vpMatrix::m_lapack_min_size) || (A.colNum > vpMatrix::m_lapack_min_size)); #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)) useLapack = false; #endif @@ -993,9 +1010,9 @@ void vpMatrix::multMatrixVector(const vpMatrix &A, const vpColVector &v, vpColVe } else { w = 0.0; - for (unsigned int j = 0; j < A.colNum; j++) { + for (unsigned int j = 0; j < A.colNum; ++j) { double vj = v[j]; // optimization em 5/12/2006 - for (unsigned int i = 0; i < A.rowNum; i++) { + for (unsigned int i = 0; i < A.rowNum; ++i) { w[i] += A.rowPtrs[i][j] * vj; } } @@ -1017,8 +1034,9 @@ void vpMatrix::multMatrixVector(const vpMatrix &A, const vpColVector &v, vpColVe */ void vpMatrix::mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C) { - if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum)) + if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum)) { C.resize(A.rowNum, B.colNum, false, false); + } if (A.colNum != B.rowNum) { throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (%dx%d) matrix", A.getRows(), @@ -1026,8 +1044,8 @@ void vpMatrix::mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C) } // If available use Lapack only for large matrices - bool useLapack = (A.rowNum > vpMatrix::m_lapack_min_size || A.colNum > vpMatrix::m_lapack_min_size || - B.colNum > vpMatrix::m_lapack_min_size); + bool useLapack = ((A.rowNum > vpMatrix::m_lapack_min_size) || (A.colNum > vpMatrix::m_lapack_min_size) || + (B.colNum > vpMatrix::m_lapack_min_size)); #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)) useLapack = false; #endif @@ -1046,13 +1064,14 @@ void vpMatrix::mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C) const unsigned int BcolNum = B.colNum; const unsigned int BrowNum = B.rowNum; double **BrowPtrs = B.rowPtrs; - for (unsigned int i = 0; i < A.rowNum; i++) { + for (unsigned int i = 0; i < A.rowNum; ++i) { const double *rowptri = A.rowPtrs[i]; double *ci = C[i]; - for (unsigned int j = 0; j < BcolNum; j++) { + for (unsigned int j = 0; j < BcolNum; ++j) { double s = 0; - for (unsigned int k = 0; k < BrowNum; k++) + for (unsigned int k = 0; k < BrowNum; ++k) { s += rowptri[k] * BrowPtrs[k][j]; + } ci[j] = s; } } @@ -1073,7 +1092,7 @@ void vpMatrix::mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C) */ void vpMatrix::mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpRotationMatrix &C) { - if (A.colNum != 3 || A.rowNum != 3 || B.colNum != 3 || B.rowNum != 3) { + if ((A.colNum != 3) || (A.rowNum != 3) || (B.colNum != 3) || (B.rowNum != 3)) { throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a " "rotation matrix", @@ -1083,13 +1102,14 @@ void vpMatrix::mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpRotationMat const unsigned int BcolNum = B.colNum; const unsigned int BrowNum = B.rowNum; double **BrowPtrs = B.rowPtrs; - for (unsigned int i = 0; i < A.rowNum; i++) { + for (unsigned int i = 0; i < A.rowNum; ++i) { const double *rowptri = A.rowPtrs[i]; double *ci = C[i]; - for (unsigned int j = 0; j < BcolNum; j++) { + for (unsigned int j = 0; j < BcolNum; ++j) { double s = 0; - for (unsigned int k = 0; k < BrowNum; k++) + for (unsigned int k = 0; k < BrowNum; ++k) { s += rowptri[k] * BrowPtrs[k][j]; + } ci[j] = s; } } @@ -1109,7 +1129,7 @@ void vpMatrix::mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpRotationMat */ void vpMatrix::mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpHomogeneousMatrix &C) { - if (A.colNum != 4 || A.rowNum != 4 || B.colNum != 4 || B.rowNum != 4) { + if ((A.colNum != 4) || (A.rowNum != 4) || (B.colNum != 4) || (B.rowNum != 4)) { throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a " "rotation matrix", @@ -1120,8 +1140,8 @@ void vpMatrix::mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpHomogeneous // Lapack usage needs to be validated again. // If available use Lapack only for large matrices. // Using SSE2 doesn't speed up. - bool useLapack = (A.rowNum > vpMatrix::m_lapack_min_size || A.colNum > vpMatrix::m_lapack_min_size || - B.colNum > vpMatrix::m_lapack_min_size); + bool useLapack = ((A.rowNum > vpMatrix::m_lapack_min_size) || (A.colNum > vpMatrix::m_lapack_min_size) || + (B.colNum > vpMatrix::m_lapack_min_size)); #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)) useLapack = false; #endif @@ -1140,13 +1160,14 @@ void vpMatrix::mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpHomogeneous const unsigned int BcolNum = B.colNum; const unsigned int BrowNum = B.rowNum; double **BrowPtrs = B.rowPtrs; - for (unsigned int i = 0; i < A.rowNum; i++) { + for (unsigned int i = 0; i < A.rowNum; ++i) { const double *rowptri = A.rowPtrs[i]; double *ci = C[i]; - for (unsigned int j = 0; j < BcolNum; j++) { + for (unsigned int j = 0; j < BcolNum; ++j) { double s = 0; - for (unsigned int k = 0; k < BrowNum; k++) + for (unsigned int k = 0; k < BrowNum; ++k) { s += rowptri[k] * BrowPtrs[k][j]; + } ci[j] = s; } } @@ -1198,13 +1219,14 @@ vpMatrix vpMatrix::operator*(const vpRotationMatrix &R) const unsigned int RcolNum = R.getCols(); unsigned int RrowNum = R.getRows(); - for (unsigned int i = 0; i < rowNum; i++) { + for (unsigned int i = 0; i < rowNum; ++i) { double *rowptri = rowPtrs[i]; double *ci = C[i]; - for (unsigned int j = 0; j < RcolNum; j++) { + for (unsigned int j = 0; j < RcolNum; ++j) { double s = 0; - for (unsigned int k = 0; k < RrowNum; k++) + for (unsigned int k = 0; k < RrowNum; ++k) { s += rowptri[k] * R[k][j]; + } ci[j] = s; } } @@ -1227,13 +1249,14 @@ vpMatrix vpMatrix::operator*(const vpHomogeneousMatrix &M) const const unsigned int McolNum = M.getCols(); const unsigned int MrowNum = M.getRows(); - for (unsigned int i = 0; i < rowNum; i++) { + for (unsigned int i = 0; i < rowNum; ++i) { const double *rowptri = rowPtrs[i]; double *ci = C[i]; - for (unsigned int j = 0; j < McolNum; j++) { + for (unsigned int j = 0; j < McolNum; ++j) { double s = 0; - for (unsigned int k = 0; k < MrowNum; k++) + for (unsigned int k = 0; k < MrowNum; ++k) { s += rowptri[k] * M[k][j]; + } ci[j] = s; } } @@ -1259,8 +1282,8 @@ vpMatrix vpMatrix::operator*(const vpVelocityTwistMatrix &V) const // Lapack usage needs to be validated again. // If available use Lapack only for large matrices. // Speed up obtained using SSE2. - bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size || - V.colNum > vpMatrix::m_lapack_min_size); + bool useLapack = ((rowNum > vpMatrix::m_lapack_min_size) || (colNum > vpMatrix::m_lapack_min_size) || + (V.colNum > vpMatrix::m_lapack_min_size)); #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)) useLapack = false; #endif @@ -1280,13 +1303,14 @@ vpMatrix vpMatrix::operator*(const vpVelocityTwistMatrix &V) const #else unsigned int VcolNum = V.getCols(); unsigned int VrowNum = V.getRows(); - for (unsigned int i = 0; i < rowNum; i++) { + for (unsigned int i = 0; i < rowNum; ++i) { double *rowptri = rowPtrs[i]; double *ci = M[i]; - for (unsigned int j = 0; j < VcolNum; j++) { + for (unsigned int j = 0; j < VcolNum; ++j) { double s = 0; - for (unsigned int k = 0; k < VrowNum; k++) + for (unsigned int k = 0; k < VrowNum; ++k) { s += rowptri[k] * V[k][j]; + } ci[j] = s; } } @@ -1314,8 +1338,8 @@ vpMatrix vpMatrix::operator*(const vpForceTwistMatrix &V) const // Lapack usage needs to be validated again. // If available use Lapack only for large matrices. // Speed up obtained using SSE2. - bool useLapack = (rowNum > vpMatrix::m_lapack_min_size || colNum > vpMatrix::m_lapack_min_size || - V.getCols() > vpMatrix::m_lapack_min_size); + bool useLapack = ((rowNum > vpMatrix::m_lapack_min_size) || (colNum > vpMatrix::m_lapack_min_size) || + (V.getCols() > vpMatrix::m_lapack_min_size)); #if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)) useLapack = false; #endif @@ -1335,13 +1359,14 @@ vpMatrix vpMatrix::operator*(const vpForceTwistMatrix &V) const #else unsigned int VcolNum = V.getCols(); unsigned int VrowNum = V.getRows(); - for (unsigned int i = 0; i < rowNum; i++) { + for (unsigned int i = 0; i < rowNum; ++i) { double *rowptri = rowPtrs[i]; double *ci = M[i]; - for (unsigned int j = 0; j < VcolNum; j++) { + for (unsigned int j = 0; j < VcolNum; ++j) { double s = 0; - for (unsigned int k = 0; k < VrowNum; k++) + for (unsigned int k = 0; k < VrowNum; ++k) { s += rowptri[k] * V[k][j]; + } ci[j] = s; } } @@ -1364,8 +1389,9 @@ vpMatrix vpMatrix::operator*(const vpForceTwistMatrix &V) const void vpMatrix::add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B, const double &wB, vpMatrix &C) { - if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum)) + if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum)) { C.resize(A.rowNum, B.colNum, false, false); + } if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) { throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(), @@ -1376,9 +1402,11 @@ void vpMatrix::add2WeightedMatrices(const vpMatrix &A, const double &wA, const v double **BrowPtrs = B.rowPtrs; double **CrowPtrs = C.rowPtrs; - for (unsigned int i = 0; i < A.rowNum; i++) - for (unsigned int j = 0; j < A.colNum; j++) - CrowPtrs[i][j] = wB * BrowPtrs[i][j] + wA * ArowPtrs[i][j]; + for (unsigned int i = 0; i < A.rowNum; ++i) { + for (unsigned int j = 0; j < A.colNum; ++j) { + CrowPtrs[i][j] = (wB * BrowPtrs[i][j]) + (wA * ArowPtrs[i][j]); + } + } } /*! @@ -1392,8 +1420,9 @@ void vpMatrix::add2WeightedMatrices(const vpMatrix &A, const double &wA, const v */ void vpMatrix::add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C) { - if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum)) + if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum)) { C.resize(A.rowNum, B.colNum, false, false); + } if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) { throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(), @@ -1404,8 +1433,8 @@ void vpMatrix::add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C) double **BrowPtrs = B.rowPtrs; double **CrowPtrs = C.rowPtrs; - for (unsigned int i = 0; i < A.rowNum; i++) { - for (unsigned int j = 0; j < A.colNum; j++) { + for (unsigned int i = 0; i < A.rowNum; ++i) { + for (unsigned int j = 0; j < A.colNum; ++j) { CrowPtrs[i][j] = BrowPtrs[i][j] + ArowPtrs[i][j]; } } @@ -1425,8 +1454,9 @@ void vpMatrix::add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C) */ void vpMatrix::add2Matrices(const vpColVector &A, const vpColVector &B, vpColVector &C) { - if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum)) + if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum)) { C.resize(A.rowNum); + } if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) { throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(), @@ -1437,8 +1467,8 @@ void vpMatrix::add2Matrices(const vpColVector &A, const vpColVector &B, vpColVec double **BrowPtrs = B.rowPtrs; double **CrowPtrs = C.rowPtrs; - for (unsigned int i = 0; i < A.rowNum; i++) { - for (unsigned int j = 0; j < A.colNum; j++) { + for (unsigned int i = 0; i < A.rowNum; ++i) { + for (unsigned int j = 0; j < A.colNum; ++j) { CrowPtrs[i][j] = BrowPtrs[i][j] + ArowPtrs[i][j]; } } @@ -1472,8 +1502,9 @@ vpMatrix vpMatrix::operator+(const vpMatrix &B) const */ void vpMatrix::sub2Matrices(const vpColVector &A, const vpColVector &B, vpColVector &C) { - if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum)) + if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum)) { C.resize(A.rowNum); + } if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) { throw(vpException(vpException::dimensionError, "Cannot subtract (%dx%d) matrix to (%dx%d) matrix", A.getRows(), @@ -1484,8 +1515,8 @@ void vpMatrix::sub2Matrices(const vpColVector &A, const vpColVector &B, vpColVec double **BrowPtrs = B.rowPtrs; double **CrowPtrs = C.rowPtrs; - for (unsigned int i = 0; i < A.rowNum; i++) { - for (unsigned int j = 0; j < A.colNum; j++) { + for (unsigned int i = 0; i < A.rowNum; ++i) { + for (unsigned int j = 0; j < A.colNum; ++j) { CrowPtrs[i][j] = ArowPtrs[i][j] - BrowPtrs[i][j]; } } @@ -1505,8 +1536,9 @@ void vpMatrix::sub2Matrices(const vpColVector &A, const vpColVector &B, vpColVec */ void vpMatrix::sub2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C) { - if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum)) + if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum)) { C.resize(A.rowNum, A.colNum, false, false); + } if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) { throw(vpException(vpException::dimensionError, "Cannot subtract (%dx%d) matrix to (%dx%d) matrix", A.getRows(), @@ -1517,8 +1549,8 @@ void vpMatrix::sub2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C) double **BrowPtrs = B.rowPtrs; double **CrowPtrs = C.rowPtrs; - for (unsigned int i = 0; i < A.rowNum; i++) { - for (unsigned int j = 0; j < A.colNum; j++) { + for (unsigned int i = 0; i < A.rowNum; ++i) { + for (unsigned int j = 0; j < A.colNum; ++j) { CrowPtrs[i][j] = ArowPtrs[i][j] - BrowPtrs[i][j]; } } @@ -1546,9 +1578,11 @@ vpMatrix &vpMatrix::operator+=(const vpMatrix &B) double **BrowPtrs = B.rowPtrs; - for (unsigned int i = 0; i < rowNum; i++) - for (unsigned int j = 0; j < colNum; j++) + for (unsigned int i = 0; i < rowNum; ++i) { + for (unsigned int j = 0; j < colNum; ++j) { rowPtrs[i][j] += BrowPtrs[i][j]; + } + } return *this; } @@ -1562,9 +1596,11 @@ vpMatrix &vpMatrix::operator-=(const vpMatrix &B) } double **BrowPtrs = B.rowPtrs; - for (unsigned int i = 0; i < rowNum; i++) - for (unsigned int j = 0; j < colNum; j++) + for (unsigned int i = 0; i < rowNum; ++i) { + for (unsigned int j = 0; j < colNum; ++j) { rowPtrs[i][j] -= BrowPtrs[i][j]; + } + } return *this; } @@ -1580,15 +1616,18 @@ vpMatrix &vpMatrix::operator-=(const vpMatrix &B) */ void vpMatrix::negateMatrix(const vpMatrix &A, vpMatrix &C) { - if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum)) + if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum)) { C.resize(A.rowNum, A.colNum, false, false); + } double **ArowPtrs = A.rowPtrs; double **CrowPtrs = C.rowPtrs; - for (unsigned int i = 0; i < A.rowNum; i++) - for (unsigned int j = 0; j < A.colNum; j++) + for (unsigned int i = 0; i < A.rowNum; ++i) { + for (unsigned int j = 0; j < A.colNum; ++j) { CrowPtrs[i][j] = -ArowPtrs[i][j]; + } + } } /*! @@ -1605,8 +1644,8 @@ vpMatrix vpMatrix::operator-() const // negate double vpMatrix::sum() const { double s = 0.0; - for (unsigned int i = 0; i < rowNum; i++) { - for (unsigned int j = 0; j < colNum; j++) { + for (unsigned int i = 0; i < rowNum; ++i) { + for (unsigned int j = 0; j < colNum; ++j) { s += rowPtrs[i][j]; } } @@ -1638,9 +1677,11 @@ vpMatrix operator*(const double &x, const vpMatrix &B) vpMatrix C; C.resize(Brow, Bcol, false, false); - for (unsigned int i = 0; i < Brow; i++) - for (unsigned int j = 0; j < Bcol; j++) + for (unsigned int i = 0; i < Brow; ++i) { + for (unsigned int j = 0; j < Bcol; ++j) { C[i][j] = B[i][j] * x; + } + } return C; } @@ -1652,15 +1693,17 @@ vpMatrix operator*(const double &x, const vpMatrix &B) vpMatrix vpMatrix::operator*(double x) const { if (std::fabs(x - 1.) < std::numeric_limits::epsilon()) { - return (*this); + return *this; } vpMatrix M; M.resize(rowNum, colNum, false, false); - for (unsigned int i = 0; i < rowNum; i++) - for (unsigned int j = 0; j < colNum; j++) + for (unsigned int i = 0; i < rowNum; ++i) { + for (unsigned int j = 0; j < colNum; ++j) { M[i][j] = rowPtrs[i][j] * x; + } + } return M; } @@ -1669,7 +1712,7 @@ vpMatrix vpMatrix::operator*(double x) const vpMatrix vpMatrix::operator/(double x) const { if (std::fabs(x - 1.) < std::numeric_limits::epsilon()) { - return (*this); + return *this; } if (std::fabs(x) < std::numeric_limits::epsilon()) { @@ -1681,9 +1724,11 @@ vpMatrix vpMatrix::operator/(double x) const double xinv = 1 / x; - for (unsigned int i = 0; i < rowNum; i++) - for (unsigned int j = 0; j < colNum; j++) + for (unsigned int i = 0; i < rowNum; ++i) { + for (unsigned int j = 0; j < colNum; ++j) { C[i][j] = rowPtrs[i][j] * xinv; + } + } return C; } @@ -1691,9 +1736,11 @@ vpMatrix vpMatrix::operator/(double x) const //! Add x to all the element of the matrix : Aij = Aij + x vpMatrix &vpMatrix::operator+=(double x) { - for (unsigned int i = 0; i < rowNum; i++) - for (unsigned int j = 0; j < colNum; j++) + for (unsigned int i = 0; i < rowNum; ++i) { + for (unsigned int j = 0; j < colNum; ++j) { rowPtrs[i][j] += x; + } + } return *this; } @@ -1701,9 +1748,11 @@ vpMatrix &vpMatrix::operator+=(double x) //! subtract x to all the element of the matrix : Aij = Aij - x vpMatrix &vpMatrix::operator-=(double x) { - for (unsigned int i = 0; i < rowNum; i++) - for (unsigned int j = 0; j < colNum; j++) + for (unsigned int i = 0; i < rowNum; ++i) { + for (unsigned int j = 0; j < colNum; ++j) { rowPtrs[i][j] -= x; + } + } return *this; } @@ -1718,9 +1767,11 @@ vpMatrix &vpMatrix::operator*=(double x) return *this; } - for (unsigned int i = 0; i < rowNum; i++) - for (unsigned int j = 0; j < colNum; j++) + for (unsigned int i = 0; i < rowNum; ++i) { + for (unsigned int j = 0; j < colNum; ++j) { rowPtrs[i][j] *= x; + } + } return *this; } @@ -1732,14 +1783,17 @@ vpMatrix &vpMatrix::operator/=(double x) return *this; } - if (std::fabs(x) < std::numeric_limits::epsilon()) + if (std::fabs(x) < std::numeric_limits::epsilon()) { throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar"); + } double xinv = 1 / x; - for (unsigned int i = 0; i < rowNum; i++) - for (unsigned int j = 0; j < colNum; j++) + for (unsigned int i = 0; i < rowNum; ++i) { + for (unsigned int j = 0; j < colNum; ++j) { rowPtrs[i][j] *= xinv; + } + } return *this; } @@ -1754,12 +1808,13 @@ vpMatrix &vpMatrix::operator/=(double x) */ void vpMatrix::stackColumns(vpColVector &out) { - if ((out.rowNum != colNum * rowNum) || (out.colNum != 1)) + if ((out.rowNum != (colNum * rowNum)) || (out.colNum != 1)) { out.resize(colNum * rowNum, false, false); + } double *optr = out.data; - for (unsigned int j = 0; j < colNum; j++) { - for (unsigned int i = 0; i < rowNum; i++) { + for (unsigned int j = 0; j < colNum; ++j) { + for (unsigned int i = 0; i < rowNum; ++i) { *(optr++) = rowPtrs[i][j]; } } @@ -1782,8 +1837,9 @@ vpColVector vpMatrix::stackColumns() */ void vpMatrix::stackRows(vpRowVector &out) { - if ((out.getRows() != 1) || (out.getCols() != colNum * rowNum)) + if ((out.getRows() != 1) || (out.getCols() != (colNum * rowNum))) { out.resize(colNum * rowNum, false, false); + } memcpy(out.data, data, sizeof(double) * out.getCols()); } @@ -1806,7 +1862,7 @@ vpRowVector vpMatrix::stackRows() */ vpMatrix vpMatrix::hadamard(const vpMatrix &m) const { - if (m.getRows() != rowNum || m.getCols() != colNum) { + if ((m.getRows() != rowNum) || (m.getCols() != colNum)) { throw(vpException(vpException::dimensionError, "In Hadamard product: bad dimension of input matrix")); } @@ -1839,14 +1895,14 @@ void vpMatrix::kron(const vpMatrix &m1, const vpMatrix &m2, vpMatrix &out) out.resize(r1 * r2, c1 * c2, false, false); - for (unsigned int r = 0; r < r1; r++) { - for (unsigned int c = 0; c < c1; c++) { + for (unsigned int r = 0; r < r1; ++r) { + for (unsigned int c = 0; c < c1; ++c) { double alpha = m1[r][c]; double *m2ptr = m2[0]; unsigned int roffset = r * r2; unsigned int coffset = c * c2; - for (unsigned int rr = 0; rr < r2; rr++) { - for (unsigned int cc = 0; cc < c2; cc++) { + for (unsigned int rr = 0; rr < r2; ++rr) { + for (unsigned int cc = 0; cc < c2; ++cc) { out[roffset + rr][coffset + cc] = alpha * *(m2ptr++); } } @@ -1878,14 +1934,14 @@ vpMatrix vpMatrix::kron(const vpMatrix &m1, const vpMatrix &m2) vpMatrix out; out.resize(r1 * r2, c1 * c2, false, false); - for (unsigned int r = 0; r < r1; r++) { - for (unsigned int c = 0; c < c1; c++) { + for (unsigned int r = 0; r < r1; ++r) { + for (unsigned int c = 0; c < c1; ++c) { double alpha = m1[r][c]; double *m2ptr = m2[0]; unsigned int roffset = r * r2; unsigned int coffset = c * c2; - for (unsigned int rr = 0; rr < r2; rr++) { - for (unsigned int cc = 0; cc < c2; cc++) { + for (unsigned int rr = 0; rr < r2; ++rr) { + for (unsigned int cc = 0; cc < c2; ++cc) { out[roffset + rr][coffset + cc] = alpha * *(m2ptr++); } } @@ -4866,12 +4922,14 @@ int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix */ vpColVector vpMatrix::getCol(unsigned int j, unsigned int i_begin, unsigned int column_size) const { - if (i_begin + column_size > getRows() || j >= getCols()) + if (((i_begin + column_size) > getRows()) || (j >= getCols())) { throw(vpException(vpException::dimensionError, "Unable to extract column %u from the %ux%u matrix", j, getRows(), getCols())); + } vpColVector c(column_size); - for (unsigned int i = 0; i < column_size; i++) + for (unsigned int i = 0; i < column_size; ++i) { c[i] = (*this)[i_begin + i][j]; + } return c; } @@ -4995,11 +5053,11 @@ vpRowVector vpMatrix::getRow(unsigned int i) const { return getRow(i, 0, colNum) */ vpRowVector vpMatrix::getRow(unsigned int i, unsigned int j_begin, unsigned int row_size) const { - if (j_begin + row_size > colNum || i >= rowNum) + if (((j_begin + row_size) > colNum) || (i >= rowNum)) { throw(vpException(vpException::dimensionError, "Unable to extract a row vector from the matrix")); - + } vpRowVector r(row_size); - if (r.data != nullptr && data != nullptr) { + if ((r.data != nullptr) && (data != nullptr)) { memcpy(r.data, (*this)[i] + j_begin, row_size * sizeof(double)); } @@ -5049,7 +5107,7 @@ vpColVector vpMatrix::getDiag() const if (min_size > 0) { diag.resize(min_size, false); - for (unsigned int i = 0; i < min_size; i++) { + for (unsigned int i = 0; i < min_size; ++i) { diag[i] = (*this)[i][i]; } } @@ -5099,24 +5157,24 @@ void vpMatrix::stack(const vpMatrix &A, const vpMatrix &B, vpMatrix &C) } } - if (A.data != nullptr && A.data == C.data) { + if ((A.data != nullptr) && (A.data == C.data)) { std::cerr << "A and C must be two different objects!" << std::endl; return; } - if (B.data != nullptr && B.data == C.data) { + if ((B.data != nullptr) && (B.data == C.data)) { std::cerr << "B and C must be two different objects!" << std::endl; return; } C.resize(nra + nrb, B.getCols(), false, false); - if (C.data != nullptr && A.data != nullptr && A.size() > 0) { + if ((C.data != nullptr) && (A.data != nullptr) && (A.size() > 0)) { // Copy A in C memcpy(C.data, A.data, sizeof(double) * A.size()); } - if (C.data != nullptr && B.data != nullptr && B.size() > 0) { + if ((C.data != nullptr) && (B.data != nullptr) && (B.size() > 0)) { // Copy B in C memcpy(C.data + A.size(), B.data, sizeof(double) * B.size()); } @@ -5152,7 +5210,7 @@ vpMatrix vpMatrix::stack(const vpMatrix &A, const vpRowVector &r) */ void vpMatrix::stack(const vpMatrix &A, const vpRowVector &r, vpMatrix &C) { - if (A.data != nullptr && A.data == C.data) { + if ((A.data != nullptr) && (A.data == C.data)) { std::cerr << "A and C must be two different objects!" << std::endl; return; } @@ -5191,7 +5249,7 @@ vpMatrix vpMatrix::stack(const vpMatrix &A, const vpColVector &c) */ void vpMatrix::stack(const vpMatrix &A, const vpColVector &c, vpMatrix &C) { - if (A.data != nullptr && A.data == C.data) { + if ((A.data != nullptr) && (A.data == C.data)) { std::cerr << "A and C must be two different objects!" << std::endl; return; } @@ -5287,7 +5345,7 @@ void vpMatrix::juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B, vpMatrix } } - if (B.getRows() == 0 || nca + ncb == 0) { + if ((B.getRows() == 0) || ((nca + ncb) == 0)) { std::cerr << "B.getRows() == 0 || nca+ncb == 0" << std::endl; return; } @@ -5348,9 +5406,9 @@ int vpMatrix::print(std::ostream &s, unsigned int length, const std::string &int oss.str(ossFixed.str()); } - values[i * n + j] = oss.str(); - size_type thislen = values[i * n + j].size(); - size_type p = values[i * n + j].find('.'); + values[(i * n) + j] = oss.str(); + size_type thislen = values[(i * n) + j].size(); + size_type p = values[(i * n) + j].find('.'); if (p == std::string::npos) { maxBefore = vpMath::maximum(maxBefore, thislen); @@ -5369,26 +5427,27 @@ int vpMatrix::print(std::ostream &s, unsigned int length, const std::string &int // decrease maxAfter according to totalLength maxAfter = std::min(maxAfter, totalLength - maxBefore); - if (!intro.empty()) + if (!intro.empty()) { s << intro; + } s << "[" << m << "," << n << "]=\n"; - for (unsigned int i = 0; i < m; i++) { + for (unsigned int i = 0; i < m; ++i) { s << " "; - for (unsigned int j = 0; j < n; j++) { - size_type p = values[i * n + j].find('.'); + for (unsigned int j = 0; j < n; ++j) { + size_type p = values[(i * n) + j].find('.'); s.setf(std::ios::right, std::ios::adjustfield); - s.width((std::streamsize)maxBefore); - s << values[i * n + j].substr(0, p).c_str(); + s.width(static_cast(maxBefore)); + s << values[(i * n) + j].substr(0, p).c_str(); if (maxAfter > 0) { s.setf(std::ios::left, std::ios::adjustfield); if (p != std::string::npos) { - s.width((std::streamsize)maxAfter); - s << values[i * n + j].substr(p, maxAfter).c_str(); + s.width(static_cast(maxAfter)); + s << values[(i * n) + j].substr(p, maxAfter).c_str(); } else { - s.width((std::streamsize)maxAfter); + s.width(static_cast(maxAfter)); s << ".0"; } } @@ -5400,7 +5459,7 @@ int vpMatrix::print(std::ostream &s, unsigned int length, const std::string &int s.flags(original_flags); // restore s to standard state - return (int)(maxBefore + maxAfter); + return static_cast(maxBefore + maxAfter); } /*! @@ -5441,12 +5500,14 @@ int vpMatrix::print(std::ostream &s, unsigned int length, const std::string &int */ std::ostream &vpMatrix::matlabPrint(std::ostream &os) const { + unsigned int this_rows = this->getRows(); + unsigned int this_col = this->getCols(); os << "[ "; - for (unsigned int i = 0; i < this->getRows(); ++i) { - for (unsigned int j = 0; j < this->getCols(); ++j) { + for (unsigned int i = 0; i < this_rows; ++i) { + for (unsigned int j = 0; j < this_col; ++j) { os << (*this)[i][j] << ", "; } - if (this->getRows() != i + 1) { + if (this->getRows() != (i + 1)) { os << ";" << std::endl; } else { @@ -5486,10 +5547,12 @@ std::ostream &vpMatrix::matlabPrint(std::ostream &os) const */ std::ostream &vpMatrix::maplePrint(std::ostream &os) const { + unsigned int this_rows = this->getRows(); + unsigned int this_col = this->getCols(); os << "([ " << std::endl; - for (unsigned int i = 0; i < this->getRows(); ++i) { + for (unsigned int i = 0; i < this_rows; ++i) { os << "["; - for (unsigned int j = 0; j < this->getCols(); ++j) { + for (unsigned int j = 0; j < this_col; ++j) { os << (*this)[i][j] << ", "; } os << "]," << std::endl; @@ -5527,11 +5590,14 @@ std::ostream &vpMatrix::maplePrint(std::ostream &os) const */ std::ostream &vpMatrix::csvPrint(std::ostream &os) const { - for (unsigned int i = 0; i < this->getRows(); ++i) { - for (unsigned int j = 0; j < this->getCols(); ++j) { + unsigned int this_rows = this->getRows(); + unsigned int this_col = this->getCols(); + for (unsigned int i = 0; i < this_rows; ++i) { + for (unsigned int j = 0; j < this_col; ++j) { os << (*this)[i][j]; - if (!(j == (this->getCols() - 1))) + if (!(j == (this->getCols() - 1))) { os << ", "; + } } os << std::endl; } @@ -5577,15 +5643,17 @@ std::ostream &vpMatrix::cppPrint(std::ostream &os, const std::string &matrixName { os << "vpMatrix " << matrixName << " (" << this->getRows() << ", " << this->getCols() << "); " << std::endl; - for (unsigned int i = 0; i < this->getRows(); ++i) { - for (unsigned int j = 0; j < this->getCols(); ++j) { + unsigned int this_rows = this->getRows(); + unsigned int this_col = this->getCols(); + for (unsigned int i = 0; i < this_rows; ++i) { + for (unsigned int j = 0; j < this_col; ++j) { if (!octet) { os << matrixName << "[" << i << "][" << j << "] = " << (*this)[i][j] << "; " << std::endl; } else { for (unsigned int k = 0; k < sizeof(double); ++k) { os << "((unsigned char*)&(" << matrixName << "[" << i << "][" << j << "]) )[" << k << "] = 0x" << std::hex - << (unsigned int)((unsigned char *)&((*this)[i][j]))[k] << "; " << std::endl; + << static_cast(((unsigned char *)&((*this)[i][j]))[k]) << "; " << std::endl; } } } @@ -5648,7 +5716,7 @@ void vpMatrix::stack(const vpRowVector &r) unsigned int oldSize = size(); resize(rowNum + 1, colNum, false, false); - if (data != nullptr && r.data != nullptr && data != r.data) { + if ((data != nullptr) && (r.data != nullptr) && (data != r.data)) { // Copy r in data memcpy(data + oldSize, r.data, sizeof(double) * r.size()); } @@ -5690,10 +5758,10 @@ void vpMatrix::stack(const vpColVector &c) unsigned int oldColNum = colNum; resize(rowNum, colNum + 1, false, false); - if (data != nullptr && tmp.data != nullptr && data != tmp.data) { + if ((data != nullptr) && (tmp.data != nullptr) && (data != tmp.data)) { // Copy c in data - for (unsigned int i = 0; i < rowNum; i++) { - memcpy(data + i * colNum, tmp.data + i * oldColNum, sizeof(double) * oldColNum); + for (unsigned int i = 0; i < rowNum; ++i) { + memcpy(data + (i * colNum), tmp.data + (i * oldColNum), sizeof(double) * oldColNum); rowPtrs[i][oldColNum] = c[i]; } } @@ -5712,13 +5780,14 @@ void vpMatrix::stack(const vpColVector &c) */ void vpMatrix::insert(const vpMatrix &A, unsigned int r, unsigned int c) { - if ((r + A.getRows()) <= rowNum && (c + A.getCols()) <= colNum) { - if (A.colNum == colNum && data != nullptr && A.data != nullptr && A.data != data) { - memcpy(data + r * colNum, A.data, sizeof(double) * A.size()); + if (((r + A.getRows()) <= rowNum) && ((c + A.getCols()) <= colNum)) { + if ((A.colNum == colNum) && (data != nullptr) && (A.data != nullptr) && (A.data != data)) { + memcpy(data + (r * colNum), A.data, sizeof(double) * A.size()); } - else if (data != nullptr && A.data != nullptr && A.data != data) { - for (unsigned int i = r; i < (r + A.getRows()); i++) { - memcpy(data + i * colNum + c, A.data + (i - r) * A.colNum, sizeof(double) * A.colNum); + else if ((data != nullptr) && (A.data != nullptr) && (A.data != data)) { + unsigned int a_rows = A.getRows(); + for (unsigned int i = r; i < (r + a_rows); ++i) { + memcpy(data + (i * colNum) + c, A.data + ((i - r) * A.colNum), sizeof(double) * A.colNum); } } } @@ -5775,9 +5844,9 @@ vpColVector vpMatrix::eigenValues() const // Check if the matrix is symmetric: At - A = 0 vpMatrix At_A = (*this).t() - (*this); - for (unsigned int i = 0; i < rowNum; i++) { - for (unsigned int j = 0; j < rowNum; j++) { - // if (At_A[i][j] != 0) { + for (unsigned int i = 0; i < rowNum; ++i) { + for (unsigned int j = 0; j < rowNum; ++j) { + // in comment: if (At_A[i][j] != 0) { if (std::fabs(At_A[i][j]) > std::numeric_limits::epsilon()) { throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symmetric matrix")); } @@ -5894,9 +5963,9 @@ void vpMatrix::eigenValues(vpColVector &evalue, vpMatrix &evector) const // Check if the matrix is symmetric: At - A = 0 vpMatrix At_A = (*this).t() - (*this); - for (unsigned int i = 0; i < rowNum; i++) { - for (unsigned int j = 0; j < rowNum; j++) { - // if (At_A[i][j] != 0) { + for (unsigned int i = 0; i < rowNum; ++i) { + for (unsigned int j = 0; j < rowNum; ++j) { + // -- in comment: if (At_A[i][j] != 0) { if (std::fabs(At_A[i][j]) > std::numeric_limits::epsilon()) { throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symmetric matrix")); } @@ -5998,10 +6067,12 @@ unsigned int vpMatrix::kernel(vpMatrix &kerAt, double svThreshold) const // kernel is computed in svd method only if the matrix has more rows than // columns - if (nbline < nbcol) + if (nbline < nbcol) { U.resize(nbcol, nbcol, true); - else + } + else { U.resize(nbline, nbcol, false); + } U.insert(*this, 0, 0); @@ -6009,26 +6080,27 @@ unsigned int vpMatrix::kernel(vpMatrix &kerAt, double svThreshold) const // Compute the highest singular value and rank of the matrix double maxsv = 0; - for (unsigned int i = 0; i < nbcol; i++) { + for (unsigned int i = 0; i < nbcol; ++i) { if (sv[i] > maxsv) { maxsv = sv[i]; } } unsigned int rank = 0; - for (unsigned int i = 0; i < nbcol; i++) { - if (sv[i] > maxsv * svThreshold) { + for (unsigned int i = 0; i < nbcol; ++i) { + if (sv[i] > (maxsv * svThreshold)) { rank++; } } kerAt.resize(nbcol - rank, nbcol); if (rank != nbcol) { - for (unsigned int j = 0, k = 0; j < nbcol; j++) { + for (unsigned int j = 0, k = 0; j < nbcol; ++j) { // if( v.col(j) in kernel and non zero ) - if ((sv[j] <= maxsv * svThreshold) && + if ((sv[j] <= (maxsv * svThreshold)) && (std::fabs(V.getCol(j).sumSquare()) > std::numeric_limits::epsilon())) { - for (unsigned int i = 0; i < V.getRows(); i++) { + unsigned int v_rows = V.getRows(); + for (unsigned int i = 0; i < v_rows; ++i) { kerAt[k][i] = V[i][j]; } k++; @@ -6069,10 +6141,12 @@ unsigned int vpMatrix::nullSpace(vpMatrix &kerA, double svThreshold) const // kernel is computed in svd method only if the matrix has more rows than // columns - if (nbrow < nbcol) + if (nbrow < nbcol) { U.resize(nbcol, nbcol, true); - else + } + else { U.resize(nbrow, nbcol, false); + } U.insert(*this, 0, 0); @@ -6082,18 +6156,18 @@ unsigned int vpMatrix::nullSpace(vpMatrix &kerA, double svThreshold) const double maxsv = sv[0]; unsigned int rank = 0; - for (unsigned int i = 0; i < nbcol; i++) { - if (sv[i] > maxsv * svThreshold) { + for (unsigned int i = 0; i < nbcol; ++i) { + if (sv[i] > (maxsv * svThreshold)) { rank++; } } kerA.resize(nbcol, nbcol - rank); if (rank != nbcol) { - for (unsigned int j = 0, k = 0; j < nbcol; j++) { + for (unsigned int j = 0, k = 0; j < nbcol; ++j) { // if( v.col(j) in kernel and non zero ) - if (sv[j] <= maxsv * svThreshold) { - for (unsigned int i = 0; i < nbcol; i++) { + if (sv[j] <= (maxsv * svThreshold)) { + for (unsigned int i = 0; i < nbcol; ++i) { kerA[i][k] = V[i][j]; } k++; @@ -6135,10 +6209,12 @@ unsigned int vpMatrix::nullSpace(vpMatrix &kerA, int dim) const // kernel is computed in svd method only if the matrix has more rows than // columns - if (nbrow < nbcol) + if (nbrow < nbcol) { U.resize(nbcol, nbcol, true); - else + } + else { U.resize(nbrow, nbcol, false); + } U.insert(*this, 0, 0); @@ -6147,9 +6223,9 @@ unsigned int vpMatrix::nullSpace(vpMatrix &kerA, int dim) const kerA.resize(nbcol, dim_); if (dim_ != 0) { unsigned int rank = nbcol - dim_; - for (unsigned int k = 0; k < dim_; k++) { + for (unsigned int k = 0; k < dim_; ++k) { unsigned int j = k + rank; - for (unsigned int i = 0; i < nbcol; i++) { + for (unsigned int i = 0; i < nbcol; ++i) { kerA[i][k] = V[i][j]; } } @@ -6157,8 +6233,8 @@ unsigned int vpMatrix::nullSpace(vpMatrix &kerA, int dim) const double maxsv = sv[0]; unsigned int rank = 0; - for (unsigned int i = 0; i < nbcol; i++) { - if (sv[i] > maxsv * 1e-6) { + for (unsigned int i = 0; i < nbcol; ++i) { + if (sv[i] > (maxsv * 1e-6)) { rank++; } } @@ -6204,7 +6280,7 @@ double vpMatrix::det(vpDetMethod method) const det = this->detByLU(); } - return (det); + return det; } /*! @@ -6237,58 +6313,60 @@ vpMatrix vpMatrix::expm() const delete[] b; return expA; #else - vpMatrix _expE(rowNum, colNum, false); - vpMatrix _expD(rowNum, colNum, false); - vpMatrix _expX(rowNum, colNum, false); - vpMatrix _expcX(rowNum, colNum, false); - vpMatrix _eye(rowNum, colNum, false); + vpMatrix v_expE(rowNum, colNum, false); + vpMatrix v_expD(rowNum, colNum, false); + vpMatrix v_expX(rowNum, colNum, false); + vpMatrix v_expcX(rowNum, colNum, false); + vpMatrix v_eye(rowNum, colNum, false); - _eye.eye(); + v_eye.eye(); vpMatrix exp(*this); - // double f; + // -- in comment: double f; int e; double c = 0.5; int q = 6; int p = 1; double nA = 0; - for (unsigned int i = 0; i < rowNum; i++) { + for (unsigned int i = 0; i < rowNum; ++i) { double sum = 0; - for (unsigned int j = 0; j < colNum; j++) { + for (unsigned int j = 0; j < colNum; ++j) { sum += fabs((*this)[i][j]); } - if (sum > nA || i == 0) { + if ((sum > nA) || (i == 0)) { nA = sum; } } /* f = */ frexp(nA, &e); - // double s = (0 > e+1)?0:e+1; + // -- in comment: double s = (0 > e+1)?0:e+1; double s = e + 1; double sca = 1.0 / pow(2.0, s); exp = sca * exp; - _expX = *this; - _expE = c * exp + _eye; - _expD = -c * exp + _eye; - for (int k = 2; k <= q; k++) { - c = c * ((double)(q - k + 1)) / ((double)(k * (2 * q - k + 1))); - _expcX = exp * _expX; - _expX = _expcX; - _expcX = c * _expX; - _expE = _expE + _expcX; - if (p) - _expD = _expD + _expcX; - else - _expD = _expD - _expcX; + v_expX = *this; + v_expE = (c * exp) + v_eye; + v_expD = (-c * exp) + v_eye; + for (int k = 2; k <= q; ++k) { + c = (c * (static_cast((q - k) + 1))) / (static_cast(k * (((2 * q) - k) + 1))); + v_expcX = exp * v_expX; + v_expX = v_expcX; + v_expcX = c * v_expX; + v_expE = v_expE + v_expcX; + if (p) { + v_expD = v_expD + v_expcX; + } + else { + v_expD = v_expD - v_expcX; + } p = !p; } - _expX = _expD.inverseByLU(); - exp = _expX * _expE; - for (int k = 1; k <= s; k++) { - _expE = exp * exp; - exp = _expE; + v_expX = v_expD.inverseByLU(); + exp = v_expX * v_expE; + for (int k = 1; k <= s; ++k) { + v_expE = exp * exp; + exp = v_expE; } return exp; #endif @@ -6319,17 +6397,23 @@ vpMatrix subblock(const vpMatrix &M, unsigned int col, unsigned int row) vpMatrix M_comp; M_comp.resize(M.getRows() - 1, M.getCols() - 1, false); - for (unsigned int i = 0; i < col; i++) { - for (unsigned int j = 0; j < row; j++) + unsigned int m_rows = M.getRows(); + unsigned int m_col = M.getCols(); + for (unsigned int i = 0; i < col; ++i) { + for (unsigned int j = 0; j < row; ++j) { M_comp[i][j] = M[i][j]; - for (unsigned int j = row + 1; j < M.getRows(); j++) + } + for (unsigned int j = row + 1; j < m_rows; ++j) { M_comp[i][j - 1] = M[i][j]; + } } - for (unsigned int i = col + 1; i < M.getCols(); i++) { - for (unsigned int j = 0; j < row; j++) + for (unsigned int i = col + 1; i < m_col; ++i) { + for (unsigned int j = 0; j < row; ++j) { M_comp[i - 1][j] = M[i][j]; - for (unsigned int j = row + 1; j < M.getRows(); j++) + } + for (unsigned int j = row + 1; j < m_rows; ++j) { M_comp[i - 1][j - 1] = M[i][j]; + } } return M_comp; } @@ -6357,10 +6441,12 @@ double vpMatrix::cond(double svThreshold) const // kernel is computed in svd method only if the matrix has more rows than // columns - if (nbline < nbcol) + if (nbline < nbcol) { U.resize(nbcol, nbcol, true); - else + } + else { U.resize(nbline, nbcol, false); + } U.insert(*this, 0, 0); @@ -6368,7 +6454,7 @@ double vpMatrix::cond(double svThreshold) const // Compute the highest singular value double maxsv = 0; - for (unsigned int i = 0; i < nbcol; i++) { + for (unsigned int i = 0; i < nbcol; ++i) { if (sv[i] > maxsv) { maxsv = sv[i]; } @@ -6376,15 +6462,15 @@ double vpMatrix::cond(double svThreshold) const // Compute the rank of the matrix unsigned int rank = 0; - for (unsigned int i = 0; i < nbcol; i++) { - if (sv[i] > maxsv * svThreshold) { + for (unsigned int i = 0; i < nbcol; ++i) { + if (sv[i] > (maxsv * svThreshold)) { rank++; } } // Compute the lowest singular value double minsv = maxsv; - for (unsigned int i = 0; i < rank; i++) { + for (unsigned int i = 0; i < rank; ++i) { if (sv[i] < minsv) { minsv = sv[i]; } @@ -6412,7 +6498,8 @@ void vpMatrix::computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM) } HLM = H; - for (unsigned int i = 0; i < H.getCols(); i++) { + unsigned int h_col = H.getCols(); + for (unsigned int i = 0; i < h_col; ++i) { HLM[i][i] += alpha * H[i][i]; } } @@ -6427,7 +6514,7 @@ void vpMatrix::computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM) double vpMatrix::frobeniusNorm() const { double norm = 0.0; - for (unsigned int i = 0; i < dsize; i++) { + for (unsigned int i = 0; i < dsize; ++i) { double x = *(data + i); norm += x * x; } @@ -6462,7 +6549,7 @@ double vpMatrix::inducedL2Norm() const // putation of the euclidean norm of the matrix is not greater than the maximum // reachable rank. Indeed, some svd library pad the singular values vector with 0s // if the input matrix is non-square. - for (unsigned int i = 0; i < boundary; i++) { + for (unsigned int i = 0; i < boundary; ++i) { if (max < w[i]) { max = w[i]; } @@ -6487,9 +6574,9 @@ double vpMatrix::inducedL2Norm() const double vpMatrix::infinityNorm() const { double norm = 0.0; - for (unsigned int i = 0; i < rowNum; i++) { + for (unsigned int i = 0; i < rowNum; ++i) { double x = 0; - for (unsigned int j = 0; j < colNum; j++) { + for (unsigned int j = 0; j < colNum; ++j) { x += fabs(*(*(rowPtrs + i) + j)); } if (x > norm) { @@ -6510,8 +6597,8 @@ double vpMatrix::sumSquare() const double sum_square = 0.0; double x; - for (unsigned int i = 0; i < rowNum; i++) { - for (unsigned int j = 0; j < colNum; j++) { + for (unsigned int i = 0; i < rowNum; ++i) { + for (unsigned int j = 0; j < colNum; ++j) { x = rowPtrs[i][j]; sum_square += x * x; } @@ -6567,8 +6654,9 @@ vpRowVector vpMatrix::row(unsigned int i) { vpRowVector c(getCols()); - for (unsigned int j = 0; j < getCols(); j++) + for (unsigned int j = 0; j < getCols(); ++j) { c[j] = (*this)[i - 1][j]; + } return c; } @@ -6593,8 +6681,9 @@ vpColVector vpMatrix::column(unsigned int j) { vpColVector c(getRows()); - for (unsigned int i = 0; i < getRows(); i++) + for (unsigned int i = 0; i < getRows(); ++i) { c[i] = (*this)[i][j - 1]; + } return c; } @@ -6606,12 +6695,16 @@ vpColVector vpMatrix::column(unsigned int j) */ void vpMatrix::setIdentity(const double &val) { - for (unsigned int i = 0; i < rowNum; i++) - for (unsigned int j = 0; j < colNum; j++) - if (i == j) + for (unsigned int i = 0; i < rowNum; ++i) { + for (unsigned int j = 0; j < colNum; ++j) { + if (i == j) { (*this)[i][j] = val; - else + } + else { (*this)[i][j] = 0; + } + } + } } #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) diff --git a/modules/core/src/math/matrix/vpMatrix_cholesky.cpp b/modules/core/src/math/matrix/vpMatrix_cholesky.cpp index cd6a591a09..9822aa5fe4 100644 --- a/modules/core/src/math/matrix/vpMatrix_cholesky.cpp +++ b/modules/core/src/math/matrix/vpMatrix_cholesky.cpp @@ -204,8 +204,8 @@ vpMatrix vpMatrix::inverseByCholeskyLapack() const throw vpMatrixException::badValue; } - for (unsigned int i = 0; i < A.getRows(); i++) - for (unsigned int j = 0; j < A.getCols(); j++) + for (unsigned int i = 0; i < A.getRows(); ++i) + for (unsigned int j = 0; j < A.getCols(); ++j) if (i > j) A[i][j] = A[j][i]; diff --git a/modules/core/src/math/matrix/vpMatrix_covariance.cpp b/modules/core/src/math/matrix/vpMatrix_covariance.cpp index 24b0565f04..f8bc53f391 100644 --- a/modules/core/src/math/matrix/vpMatrix_covariance.cpp +++ b/modules/core/src/math/matrix/vpMatrix_covariance.cpp @@ -60,11 +60,12 @@ vpMatrix vpMatrix::computeCovarianceMatrix(const vpMatrix &A, const vpColVector { // double denom = ((double)(A.getRows()) - (double)(A.getCols())); // To // consider OLS Estimate for sigma - double denom = ((double)(A.getRows())); // To consider MLE Estimate for sigma + double denom = (static_cast(A.getRows())); // To consider MLE Estimate for sigma - if (denom <= std::numeric_limits::epsilon()) + if (denom <= std::numeric_limits::epsilon()) { throw vpMatrixException(vpMatrixException::divideByZeroError, "Impossible to compute covariance matrix: not enough data"); + } // double sigma2 = ( ((b.t())*b) - ( (b.t())*A*x ) ); // Should be // equivalent to line bellow. @@ -93,18 +94,20 @@ vpMatrix vpMatrix::computeCovarianceMatrix(const vpMatrix &A, const vpColVector { double denom = 0.0; vpMatrix W2(W.getCols(), W.getCols()); - for (unsigned int i = 0; i < W.getCols(); i++) { + unsigned int w_cols = W.getCols(); + for (unsigned int i = 0; i < w_cols; ++i) { denom += W[i][i]; W2[i][i] = W[i][i] * W[i][i]; } - if (denom <= std::numeric_limits::epsilon()) + if (denom <= std::numeric_limits::epsilon()) { throw vpMatrixException(vpMatrixException::divideByZeroError, "Impossible to compute covariance matrix: not enough data"); + } // double sigma2 = ( ((W*b).t())*W*b - ( ((W*b).t())*W*A*x ) ); // Should // be equivalent to line bellow. - double sigma2 = (W * b - (W * A * x)).t() * (W * b - (W * A * x)); + double sigma2 = ((W * b) - (W * A * x)).t() * (W * b - (W * A * x)); sigma2 /= denom; return (A.t() * (W2)*A).pseudoInverse(A.getCols() * std::numeric_limits::epsilon()) * sigma2; @@ -175,49 +178,54 @@ void vpMatrix::computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const cMo.extract(thetau); vpColVector tu(3); - for (unsigned int i = 0; i < 3; i++) + for (unsigned int i = 0; i < 3; ++i) { tu[i] = thetau[i]; + } double theta = sqrt(tu.sumSquare()); - // vpMatrix Lthetau(3,3); + // --comment: vpMatrix Lthetau(3,3); vpMatrix LthetauInvAnalytic(3, 3); vpMatrix I3(3, 3); I3.eye(); - // Lthetau = -I3; + // --comment: Lthetau = -I3; LthetauInvAnalytic = -I3; - if (theta / (2.0 * M_PI) > std::numeric_limits::epsilon()) { + if ((theta / (2.0 * M_PI)) > std::numeric_limits::epsilon()) { // Computing [theta/2 u]_x vpColVector theta2u(3); - for (unsigned int i = 0; i < 3; i++) { + for (unsigned int i = 0; i < 3; ++i) { theta2u[i] = tu[i] / 2.0; } vpMatrix theta2u_skew = vpColVector::skew(theta2u); vpColVector u(3); - for (unsigned int i = 0; i < 3; i++) { + for (unsigned int i = 0; i < 3; ++i) { u[i] = tu[i] / theta; } vpMatrix u_skew = vpColVector::skew(u); - // Lthetau += (theta2u_skew - - // (1.0-vpMath::sinc(theta)/vpMath::sqr(vpMath::sinc(theta/2.0)))*u_skew*u_skew); + // --comment: Lthetau += (theta2u_skew - + // --comment: (1.0-vpMath::sinc(theta)/vpMath::sqr(vpMath::sinc(theta/2.0)))*u_skew*u_skew); LthetauInvAnalytic += - -(vpMath::sqr(vpMath::sinc(theta / 2.0)) * theta2u_skew - (1.0 - vpMath::sinc(theta)) * u_skew * u_skew); + -((vpMath::sqr(vpMath::sinc(theta / 2.0)) * theta2u_skew) - ((1.0 - vpMath::sinc(theta)) * u_skew * u_skew)); } - // vpMatrix LthetauInv = Lthetau.inverseByLU(); + // --comment: vpMatrix LthetauInv = Lthetau.inverseByLU(); ctoInitSkew = ctoInitSkew * LthetauInvAnalytic; - for (unsigned int a = 0; a < 3; a++) - for (unsigned int b = 0; b < 3; b++) + for (unsigned int a = 0; a < 3; ++a) { + for (unsigned int b = 0; b < 3; ++b) { LpInv[a][b + 3] = ctoInitSkew[a][b]; + } + } - for (unsigned int a = 0; a < 3; a++) - for (unsigned int b = 0; b < 3; b++) + for (unsigned int a = 0; a < 3; ++a) { + for (unsigned int b = 0; b < 3; ++b) { LpInv[a + 3][b + 3] = LthetauInvAnalytic[a][b]; + } + } // Building Js Js = Ls * LpInv; diff --git a/modules/core/src/math/matrix/vpMatrix_lu.cpp b/modules/core/src/math/matrix/vpMatrix_lu.cpp index b17219e062..5e72871d33 100644 --- a/modules/core/src/math/matrix/vpMatrix_lu.cpp +++ b/modules/core/src/math/matrix/vpMatrix_lu.cpp @@ -126,7 +126,7 @@ int main() */ vpMatrix vpMatrix::inverseByLU() const { - if (colNum == 1 && rowNum == 1) { + if ((colNum == 1) && (rowNum == 1)) { vpMatrix inv; inv.resize(1, 1, false); double d = det(); @@ -136,7 +136,8 @@ vpMatrix vpMatrix::inverseByLU() const } inv[0][0] = 1. / d; return inv; - } else if (colNum == 2 && rowNum == 2) { + } + else if ((colNum == 2) && (rowNum == 2)) { vpMatrix inv; inv.resize(2, 2, false); double d = det(); @@ -150,7 +151,8 @@ vpMatrix vpMatrix::inverseByLU() const inv[0][1] = -(*this)[0][1] * d; inv[1][0] = -(*this)[1][0] * d; return inv; - } else if (colNum == 3 && rowNum == 3) { + } + else if ((colNum == 3) && (rowNum == 3)) { vpMatrix inv; inv.resize(3, 3, false); double d = det(); @@ -159,19 +161,20 @@ vpMatrix vpMatrix::inverseByLU() const rowNum, colNum)); } d = 1. / d; - inv[0][0] = ((*this)[1][1] * (*this)[2][2] - (*this)[1][2] * (*this)[2][1]) * d; - inv[0][1] = ((*this)[0][2] * (*this)[2][1] - (*this)[0][1] * (*this)[2][2]) * d; - inv[0][2] = ((*this)[0][1] * (*this)[1][2] - (*this)[0][2] * (*this)[1][1]) * d; + inv[0][0] = (((*this)[1][1] * (*this)[2][2]) - ((*this)[1][2] * (*this)[2][1])) * d; + inv[0][1] = (((*this)[0][2] * (*this)[2][1]) - ((*this)[0][1] * (*this)[2][2])) * d; + inv[0][2] = (((*this)[0][1] * (*this)[1][2]) - ((*this)[0][2] * (*this)[1][1])) * d; - inv[1][0] = ((*this)[1][2] * (*this)[2][0] - (*this)[1][0] * (*this)[2][2]) * d; - inv[1][1] = ((*this)[0][0] * (*this)[2][2] - (*this)[0][2] * (*this)[2][0]) * d; - inv[1][2] = ((*this)[0][2] * (*this)[1][0] - (*this)[0][0] * (*this)[1][2]) * d; + inv[1][0] = (((*this)[1][2] * (*this)[2][0]) - ((*this)[1][0] * (*this)[2][2])) * d; + inv[1][1] = (((*this)[0][0] * (*this)[2][2]) - ((*this)[0][2] * (*this)[2][0])) * d; + inv[1][2] = (((*this)[0][2] * (*this)[1][0]) - ((*this)[0][0] * (*this)[1][2])) * d; - inv[2][0] = ((*this)[1][0] * (*this)[2][1] - (*this)[1][1] * (*this)[2][0]) * d; - inv[2][1] = ((*this)[0][1] * (*this)[2][0] - (*this)[0][0] * (*this)[2][1]) * d; - inv[2][2] = ((*this)[0][0] * (*this)[1][1] - (*this)[0][1] * (*this)[1][0]) * d; + inv[2][0] = (((*this)[1][0] * (*this)[2][1]) - ((*this)[1][1] * (*this)[2][0])) * d; + inv[2][1] = (((*this)[0][1] * (*this)[2][0]) - ((*this)[0][0] * (*this)[2][1])) * d; + inv[2][2] = (((*this)[0][0] * (*this)[1][1]) - ((*this)[0][1] * (*this)[1][0])) * d; return inv; - } else { + } + else { #if defined(VISP_HAVE_LAPACK) return inverseByLULapack(); #elif defined(VISP_HAVE_EIGEN3) @@ -180,7 +183,7 @@ vpMatrix vpMatrix::inverseByLU() const return inverseByLUOpenCV(); #else throw(vpException(vpException::fatalError, "Cannot inverse by LU. " - "Install Lapack, Eigen3 or OpenCV 3rd party")); + "Install Lapack, Eigen3 or OpenCV 3rd party")); #endif } } @@ -220,15 +223,18 @@ int main() */ double vpMatrix::detByLU() const { - if (rowNum == 1 && colNum == 1) { + if ((rowNum == 1) && (colNum == 1)) { return (*this)[0][0]; - } else if (rowNum == 2 && colNum == 2) { - return ((*this)[0][0] * (*this)[1][1] - (*this)[0][1] * (*this)[1][0]); - } else if (rowNum == 3 && colNum == 3) { + } + else if ((rowNum == 2) && (colNum == 2)) { + return (((*this)[0][0] * (*this)[1][1]) - ((*this)[0][1] * (*this)[1][0])); + } + else if ((rowNum == 3) && (colNum == 3)) { return ((*this)[0][0] * ((*this)[1][1] * (*this)[2][2] - (*this)[1][2] * (*this)[2][1]) - (*this)[0][1] * ((*this)[1][0] * (*this)[2][2] - (*this)[1][2] * (*this)[2][0]) + (*this)[0][2] * ((*this)[1][0] * (*this)[2][1] - (*this)[1][1] * (*this)[2][0])); - } else { + } + else { #if defined(VISP_HAVE_LAPACK) return detByLULapack(); #elif defined(VISP_HAVE_EIGEN3) @@ -237,7 +243,7 @@ double vpMatrix::detByLU() const return detByLUOpenCV(); #else throw(vpException(vpException::fatalError, "Cannot compute matrix determinant. " - "Install Lapack, Eigen3 or OpenCV 3rd party")); + "Install Lapack, Eigen3 or OpenCV 3rd party")); #endif } } @@ -284,10 +290,10 @@ vpMatrix vpMatrix::inverseByLULapack() const gsl_matrix *A = gsl_matrix_alloc(rowNum, colNum); // copy the input matrix to ensure the function doesn't modify its content - unsigned int tda = (unsigned int)A->tda; - for (unsigned int i = 0; i < rowNum; i++) { + unsigned int tda = static_cast(A->tda); + for (unsigned int i = 0; i < rowNum; ++i) { unsigned int k = i * tda; - for (unsigned int j = 0; j < colNum; j++) + for (unsigned int j = 0; j < colNum; ++j) A->data[k + j] = (*this)[i][j]; } @@ -424,12 +430,12 @@ double vpMatrix::detByLULapack() const } double det = A[0][0]; - for (unsigned int i = 1; i < rowNum; i++) { + for (unsigned int i = 1; i < rowNum; ++i) { det *= A[i][i]; } double sign = 1.; - for (int i = 1; i <= dim; i++) { + for (int i = 1; i <= dim; ++i) { if (ipiv[i] != i) sign = -sign; } diff --git a/modules/core/src/math/matrix/vpMatrix_qr.cpp b/modules/core/src/math/matrix/vpMatrix_qr.cpp index a172a21ed3..4c362323f3 100644 --- a/modules/core/src/math/matrix/vpMatrix_qr.cpp +++ b/modules/core/src/math/matrix/vpMatrix_qr.cpp @@ -107,9 +107,9 @@ int allocate_work(double **work) void display_gsl(gsl_matrix *M) { // display - for (unsigned int i = 0; i < M->size1; i++) { + for (unsigned int i = 0; i < M->size1; ++i) { unsigned int k = i * M->tda; - for (unsigned int j = 0; j < M->size2; j++) { + for (unsigned int j = 0; j < M->size2; ++j) { std::cout << M->data[k + j] << " "; } std::cout << std::endl; @@ -172,7 +172,7 @@ vpMatrix vpMatrix::inverseByQRLapack() const // copy input matrix since gsl_linalg_QR_decomp() is destructive unsigned int Atda = static_cast(gsl_A->tda); size_t len = sizeof(double) * colNum; - for (unsigned int i = 0; i < rowNum; i++) { + for (unsigned int i = 0; i < rowNum; ++i) { unsigned int k = i * Atda; memcpy(&gsl_A->data[k], (*this)[i], len); } @@ -184,7 +184,7 @@ vpMatrix vpMatrix::inverseByQRLapack() const { gsl_matrix_view m; gsl_vector_view v; - for (unsigned int i = 0; i < rowNum; i++) { + for (unsigned int i = 0; i < rowNum; ++i) { double *Tii = gsl_matrix_ptr(gsl_R, i, i); *Tii = 1.0 / (*Tii); double aii = -(*Tii); @@ -202,7 +202,7 @@ vpMatrix vpMatrix::inverseByQRLapack() const gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, gsl_R, gsl_Q, 0, &gsl_inv); unsigned int gsl_inv_tda = static_cast(gsl_inv.tda); size_t inv_len = sizeof(double) * inv.colNum; - for (unsigned int i = 0; i < inv.rowNum; i++) { + for (unsigned int i = 0; i < inv.rowNum; ++i) { unsigned int k = i * gsl_inv_tda; memcpy(inv[i], &gsl_inv.data[k], inv_len); } @@ -306,8 +306,8 @@ vpMatrix vpMatrix::inverseByQRLapack() const // the matrix is upper triangular for lapack but lower triangular for visp // we fill it with zeros above the diagonal (where we don't need the // values) - for (unsigned int i = 0; i < C.getRows(); i++) - for (unsigned int j = 0; j < C.getRows(); j++) + for (unsigned int i = 0; i < C.getRows(); ++i) + for (unsigned int j = 0; j < C.getRows(); ++j) if (j > i) C[i][j] = 0.; @@ -476,10 +476,10 @@ unsigned int vpMatrix::qr(vpMatrix &Q, vpMatrix &R, bool full, bool squareR, dou // copy input matrix since gsl_linalg_QR_decomp() is destructive unsigned int Atda = static_cast(gsl_A->tda); size_t len = sizeof(double) * colNum; - for (unsigned int i = 0; i < rowNum; i++) { + for (unsigned int i = 0; i < rowNum; ++i) { unsigned int k = i * Atda; memcpy(&gsl_A->data[k], (*this)[i], len); - // for (unsigned int j = 0; j < colNum; j++) + // for (unsigned int j = 0; j < colNum; ++j) // gsl_A->data[k + j] = (*this)[i][j]; } @@ -508,10 +508,10 @@ unsigned int vpMatrix::qr(vpMatrix &Q, vpMatrix &R, bool full, bool squareR, dou unsigned int Qtda = static_cast(gsl_Q->tda); size_t len = sizeof(double) * Q.colNum; - for (unsigned int i = 0; i < Q.rowNum; i++) { + for (unsigned int i = 0; i < Q.rowNum; ++i) { unsigned int k = i * Qtda; memcpy(Q[i], &gsl_Q->data[k], len); - // for(unsigned int j = 0; j < Q.colNum; j++) { + // for(unsigned int j = 0; j < Q.colNum; ++j) { // Q[i][j] = gsl_Q->data[k + j]; // } } @@ -522,10 +522,10 @@ unsigned int vpMatrix::qr(vpMatrix &Q, vpMatrix &R, bool full, bool squareR, dou na = std::min(m, n); unsigned int Rtda = static_cast(gsl_R->tda); size_t Rlen = sizeof(double) * R.colNum; - for (unsigned int i = 0; i < na; i++) { + for (unsigned int i = 0; i < na; ++i) { unsigned int k = i * Rtda; memcpy(R[i], &gsl_R->data[k], Rlen); - // for(unsigned int j = i; j < na; j++) + // for(unsigned int j = i; j < na; ++j) // R[i][j] = gsl_R->data[k + j]; if (std::abs(gsl_R->data[k + i]) < tol) r--; @@ -614,16 +614,16 @@ unsigned int vpMatrix::qr(vpMatrix &Q, vpMatrix &R, bool full, bool squareR, dou // copy useful part of R from Q and update rank na = std::min(m, n); if (squareR) { - for (int i = 0; i < na; i++) { - for (int j = i; j < na; j++) + for (int i = 0; i < na; ++i) { + for (int j = i; j < na; ++j) R[i][j] = qrdata[i + m * j]; if (std::abs(qrdata[i + m * i]) < tol) r--; } } else { - for (int i = 0; i < na; i++) { - for (int j = i; j < n; j++) + for (int i = 0; i < na; ++i) { + for (int j = i; j < n; ++j) R[i][j] = qrdata[i + m * j]; if (std::abs(qrdata[i + m * i]) < tol) r--; @@ -794,10 +794,10 @@ unsigned int vpMatrix::qrPivot(vpMatrix &Q, vpMatrix &R, vpMatrix &P, bool full, unsigned int Qtda = static_cast(gsl_Q->tda); size_t len = sizeof(double) * Q.colNum; - for (unsigned int i = 0; i < Q.rowNum; i++) { + for (unsigned int i = 0; i < Q.rowNum; ++i) { unsigned int k = i * Qtda; memcpy(Q[i], &gsl_Q->data[k], len); - // for(unsigned int j = 0; j < Q.colNum; j++) { + // for(unsigned int j = 0; j < Q.colNum; ++j) { // Q[i][j] = gsl_Q->data[k + j]; // } } @@ -807,7 +807,7 @@ unsigned int vpMatrix::qrPivot(vpMatrix &Q, vpMatrix &R, vpMatrix &P, bool full, // update rank na = std::min(m, n); unsigned int Rtda = static_cast(gsl_R->tda); - for (unsigned int i = 0; i < na; i++) { + for (unsigned int i = 0; i < na; ++i) { unsigned int k = i * Rtda; if (std::abs(gsl_R->data[k + i]) < tol) r--; @@ -828,7 +828,7 @@ unsigned int vpMatrix::qrPivot(vpMatrix &Q, vpMatrix &R, vpMatrix &P, bool full, // copy useful part of R size_t Rlen = sizeof(double) * R.colNum; - for (unsigned int i = 0; i < na; i++) { + for (unsigned int i = 0; i < na; ++i) { unsigned int k = i * Rtda; memcpy(R[i], &gsl_R->data[k], Rlen); } @@ -942,8 +942,8 @@ unsigned int vpMatrix::qrPivot(vpMatrix &Q, vpMatrix &R, vpMatrix &P, bool full, if (squareR) // R r x r { R.resize(static_cast(r), static_cast(r)); - for (int i = 0; i < r; i++) - for (int j = i; j < r; j++) + for (int i = 0; i < r; ++i) + for (int j = i; j < r; ++j) R[i][j] = qrdata[i + m * j]; // write P @@ -954,8 +954,8 @@ unsigned int vpMatrix::qrPivot(vpMatrix &Q, vpMatrix &R, vpMatrix &P, bool full, else // R is min(m,n) x n of rank r { R.resize(static_cast(na), static_cast(n)); - for (int i = 0; i < na; i++) - for (int j = i; j < n; j++) + for (int i = 0; i < na; ++i) + for (int j = i; j < n; ++j) R[i][j] = qrdata[i + m * j]; // write P P.resize(static_cast(n), static_cast(n)); @@ -1028,7 +1028,7 @@ vpMatrix vpMatrix::inverseTriangular(bool upper) const unsigned int tda = static_cast(gsl_inv.tda); size_t len = sizeof(double) * inv.colNum; - for (unsigned int i = 0; i < rowNum; i++) { + for (unsigned int i = 0; i < rowNum; ++i) { unsigned int k = i * tda; memcpy(&gsl_inv.data[k], (*this)[i], len); } @@ -1040,7 +1040,7 @@ vpMatrix vpMatrix::inverseTriangular(bool upper) const { gsl_matrix_view m; gsl_vector_view v; - for (unsigned int i = 0; i < rowNum; i++) { + for (unsigned int i = 0; i < rowNum; ++i) { double *Tii = gsl_matrix_ptr(&gsl_inv, i, i); *Tii = 1.0 / *Tii; double aii = -(*Tii); @@ -1061,7 +1061,7 @@ vpMatrix vpMatrix::inverseTriangular(bool upper) const { gsl_matrix_view m; gsl_vector_view v; - for (unsigned int i = 0; i < rowNum; i++) { + for (unsigned int i = 0; i < rowNum; ++i) { size_t j = rowNum - i - 1; double *Tjj = gsl_matrix_ptr(&gsl_inv, j, j); *Tjj = 1.0 / (*Tjj); diff --git a/modules/core/src/math/matrix/vpRowVector.cpp b/modules/core/src/math/matrix/vpRowVector.cpp index a3a88349d4..464ff165c8 100644 --- a/modules/core/src/math/matrix/vpRowVector.cpp +++ b/modules/core/src/math/matrix/vpRowVector.cpp @@ -83,8 +83,9 @@ vpRowVector &vpRowVector::operator=(const vpMatrix &M) M.getCols(), M.getRows(), M.getCols())); } - if (M.getCols() != colNum) + if (M.getCols() != colNum) { resize(M.getCols()); + } memcpy(data, M.data, colNum * sizeof(double)); return *this; @@ -95,9 +96,11 @@ vpRowVector &vpRowVector::operator=(const vpMatrix &M) */ vpRowVector &vpRowVector::operator=(const std::vector &v) { - resize((unsigned int)v.size()); - for (unsigned int i = 0; i < v.size(); i++) + unsigned int v_size = v.size(); + resize(v_size); + for (unsigned int i = 0; i < v_size; ++i) { (*this)[i] = v[i]; + } return *this; } /*! @@ -105,17 +108,19 @@ vpRowVector &vpRowVector::operator=(const std::vector &v) */ vpRowVector &vpRowVector::operator=(const std::vector &v) { - resize((unsigned int)v.size()); - for (unsigned int i = 0; i < v.size(); i++) - (*this)[i] = (float)v[i]; + unsigned int v_size = v.size(); + resize(v_size); + for (unsigned int i = 0; i < v_size; ++i) { + (*this)[i] = static_cast(v[i]); + } return *this; } //! Initialize each element of the vector with \e x. vpRowVector &vpRowVector::operator=(double x) { - for (unsigned int i = 0; i < rowNum; i++) { - for (unsigned int j = 0; j < colNum; j++) { + for (unsigned int i = 0; i < rowNum; ++i) { + for (unsigned int j = 0; j < colNum; ++j) { rowPtrs[i][j] = x; } } @@ -173,12 +178,14 @@ vpRowVector &vpRowVector::operator=(const std::initializer_list &list) bool vpRowVector::operator==(const vpRowVector &v) const { - if (colNum != v.colNum || rowNum != v.rowNum /* should not happen */) + if ((colNum != v.colNum) || (rowNum != v.rowNum) /* should not happen */) { return false; + } - for (unsigned int i = 0; i < colNum; i++) { - if (!vpMath::equal(data[i], v.data[i], std::numeric_limits::epsilon())) + for (unsigned int i = 0; i < colNum; ++i) { + if (!vpMath::equal(data[i], v.data[i], std::numeric_limits::epsilon())) { return false; + } } return true; @@ -210,7 +217,7 @@ double vpRowVector::operator*(const vpColVector &x) const double scalar = 0.0; - for (unsigned int i = 0; i < nelements; i++) { + for (unsigned int i = 0; i < nelements; ++i) { scalar += (*this)[i] * x[i]; } return scalar; @@ -241,9 +248,10 @@ vpRowVector vpRowVector::operator*(const vpMatrix &M) const c = 0.0; - for (unsigned int i = 0; i < colNum; i++) { + for (unsigned int i = 0; i < colNum; ++i) { double bi = data[i]; // optimization em 5/12/2006 - for (unsigned int j = 0; j < M.getCols(); j++) { + unsigned int m_cols = M.getCols(); + for (unsigned int j = 0; j < m_cols; ++j) { c[j] += bi * M[i][j]; } } @@ -277,8 +285,9 @@ vpRowVector vpRowVector::operator*(double x) const double *vd = v.data; double *d = data; - for (unsigned int i = 0; i < colNum; i++) + for (unsigned int i = 0; i < colNum; ++i) { *(vd++) = (*d++) * x; + } return v; } @@ -301,8 +310,9 @@ vpRowVector vpRowVector::operator*(double x) const */ vpRowVector &vpRowVector::operator*=(double x) { - for (unsigned int i = 0; i < colNum; i++) + for (unsigned int i = 0; i < colNum; ++i) { (*this)[i] *= x; + } return (*this); } @@ -332,8 +342,9 @@ vpRowVector vpRowVector::operator/(double x) const double *vd = v.data; double *d = data; - for (unsigned int i = 0; i < colNum; i++) + for (unsigned int i = 0; i < colNum; ++i) { *(vd++) = (*d++) / x; + } return v; } @@ -357,8 +368,9 @@ vpRowVector vpRowVector::operator/(double x) const */ vpRowVector &vpRowVector::operator/=(double x) { - for (unsigned int i = 0; i < colNum; i++) + for (unsigned int i = 0; i < colNum; ++i) { (*this)[i] /= x; + } return (*this); } @@ -379,8 +391,9 @@ vpRowVector vpRowVector::operator-() const double *vd = A.data; double *d = data; - for (unsigned int i = 0; i < colNum; i++) + for (unsigned int i = 0; i < colNum; ++i) { *(vd++) = -(*d++); + } return A; } @@ -398,8 +411,9 @@ vpRowVector vpRowVector::operator-(const vpRowVector &m) const vpRowVector v(colNum); - for (unsigned int i = 0; i < colNum; i++) + for (unsigned int i = 0; i < colNum; ++i) { v[i] = (*this)[i] - m[i]; + } return v; } @@ -416,8 +430,9 @@ vpRowVector vpRowVector::operator+(const vpRowVector &v) const vpRowVector r(colNum); - for (unsigned int i = 0; i < colNum; i++) + for (unsigned int i = 0; i < colNum; ++i) { r[i] = (*this)[i] + v[i]; + } return r; } @@ -433,8 +448,9 @@ vpRowVector &vpRowVector::operator+=(vpRowVector v) v.getCols())); } - for (unsigned int i = 0; i < colNum; i++) + for (unsigned int i = 0; i < colNum; ++i) { (*this)[i] += v[i]; + } return (*this); } @@ -450,8 +466,9 @@ vpRowVector &vpRowVector::operator-=(vpRowVector v) v.getCols())); } - for (unsigned int i = 0; i < colNum; i++) + for (unsigned int i = 0; i < colNum; ++i) { (*this)[i] -= v[i]; + } return (*this); } @@ -523,8 +540,10 @@ void vpRowVector::transpose(vpColVector &v) const { v = t(); } */ vpRowVector::vpRowVector(const vpMatrix &M, unsigned int i) : vpArray2D(1, M.getCols()) { - for (unsigned int j = 0; j < M.getCols(); j++) + unsigned int m_cols = M.getCols(); + for (unsigned int j = 0; j < m_cols; ++j) { (*this)[j] = M[i][j]; + } } /*! Constructor that creates a row vector from a 1-by-n matrix \e M. @@ -538,26 +557,31 @@ vpRowVector::vpRowVector(const vpMatrix &M) : vpArray2D(1, M.getCols()) throw(vpException(vpException::dimensionError, "Cannot construct a (1x%d) row vector from a (%dx%d) matrix", M.getCols(), M.getRows(), M.getCols())); } - - for (unsigned int j = 0; j < M.getCols(); j++) + unsigned int m_cols = M.getCols(); + for (unsigned int j = 0; j < m_cols; ++j) { (*this)[j] = M[0][j]; + } } /*! Constructor that creates a row vector from a std vector of double. */ -vpRowVector::vpRowVector(const std::vector &v) : vpArray2D(1, (unsigned int)v.size()) +vpRowVector::vpRowVector(const std::vector &v) : vpArray2D(1, static_cast(v.size())) { - for (unsigned int j = 0; j < v.size(); j++) + unsigned int v_size = v.size(); + for (unsigned int j = 0; j < v_size; ++j) { (*this)[j] = v[j]; + } } /*! Constructor that creates a row vector from a std vector of float. */ -vpRowVector::vpRowVector(const std::vector &v) : vpArray2D(1, (unsigned int)v.size()) +vpRowVector::vpRowVector(const std::vector &v) : vpArray2D(1, static_cast(v.size())) { - for (unsigned int j = 0; j < v.size(); j++) - (*this)[j] = (double)(v[j]); + unsigned int v_size = v.size(); + for (unsigned int j = 0; j < v_size; ++j) { + (*this)[j] = static_cast(v[j]); + } } /*! @@ -693,20 +717,23 @@ row vector: 1 2 3 4 5 6 7 8 9 10 11 12 */ void vpRowVector::reshape(vpMatrix &M, const unsigned int &nrows, const unsigned int &ncols) { - if (dsize != nrows * ncols) { + if (dsize != (nrows * ncols)) { throw(vpException(vpException::dimensionError, "Cannot reshape (1x%d) row vector in (%dx%d) matrix", colNum, M.getRows(), M.getCols())); } try { - if ((M.getRows() != nrows) || (M.getCols() != ncols)) + if ((M.getRows() != nrows) || (M.getCols() != ncols)) { M.resize(nrows, ncols); + } } catch (...) { throw; } - for (unsigned int i = 0; i < nrows; i++) - for (unsigned int j = 0; j < ncols; j++) - M[i][j] = data[i * ncols + j]; + for (unsigned int i = 0; i < nrows; ++i) { + for (unsigned int j = 0; j < ncols; ++j) { + M[i][j] = data[(i * ncols) + j]; + } + } } /*! @@ -742,13 +769,16 @@ v: 0 10 11 3 */ void vpRowVector::insert(unsigned int i, const vpRowVector &v) { - if (i + v.size() > this->size()) + if ( (i + v.size()) > this->size()) { throw(vpException(vpException::dimensionError, "Unable to insert (1x%d) row vector in (1x%d) row " "vector at position (%d)", v.getCols(), colNum, i)); - for (unsigned int j = 0; j < v.size(); j++) + } + unsigned int v_size = v.size(); + for (unsigned int j = 0; j < v_size; ++j) { (*this)[i + j] = v[j]; + } } /*! @@ -759,8 +789,10 @@ std::vector vpRowVector::toStdVector() const { std::vector v(this->size()); - for (unsigned int i = 0; i < this->size(); i++) + unsigned int this_size = this->size(); + for (unsigned int i = 0; i < this_size; ++i) { v[i] = data[i]; + } return v; } @@ -859,7 +891,7 @@ void vpRowVector::stack(const vpRowVector &A, const vpRowVector &B, vpRowVector unsigned int nrA = A.getCols(); unsigned int nrB = B.getCols(); - if (nrA == 0 && nrB == 0) { + if ((nrA == 0) && (nrB == 0)) { C.resize(0); return; } @@ -877,11 +909,13 @@ void vpRowVector::stack(const vpRowVector &A, const vpRowVector &B, vpRowVector // General case C.resize(nrA + nrB); - for (unsigned int i = 0; i < nrA; i++) + for (unsigned int i = 0; i < nrA; ++i) { C[i] = A[i]; + } - for (unsigned int i = 0; i < nrB; i++) + for (unsigned int i = 0; i < nrB; ++i) { C[nrA + i] = B[i]; + } } /*! @@ -889,14 +923,16 @@ void vpRowVector::stack(const vpRowVector &A, const vpRowVector &B, vpRowVector */ double vpRowVector::mean(const vpRowVector &v) { - if (v.data == nullptr || v.size() == 0) { + if ((v.data == nullptr) || (v.size() == 0)) { throw(vpException(vpException::dimensionError, "Cannot compute mean value of an empty row vector")); } double mean = 0; double *vd = v.data; - for (unsigned int i = 0; i < v.getCols(); i++) + unsigned int v_col = v.getCols(); + for (unsigned int i = 0; i < v_col; ++i) { mean += *(vd++); + } return mean / v.getCols(); } @@ -906,7 +942,7 @@ double vpRowVector::mean(const vpRowVector &v) */ double vpRowVector::median(const vpRowVector &v) { - if (v.data == nullptr || v.size() == 0) { + if ((v.data == nullptr) || (v.size() == 0)) { throw(vpException(vpException::dimensionError, "Cannot compute mean value of an empty row vector")); } @@ -920,18 +956,19 @@ double vpRowVector::median(const vpRowVector &v) */ double vpRowVector::stdev(const vpRowVector &v, bool useBesselCorrection) { - if (v.data == nullptr || v.size() == 0) { + if ((v.data == nullptr) || (v.size() == 0)) { throw(vpException(vpException::dimensionError, "Cannot compute mean value of an empty row vector")); } double mean_value = mean(v); double sum_squared_diff = 0.0; - for (unsigned int i = 0; i < v.size(); i++) { + unsigned int v_size = v.size(); + for (unsigned int i = 0; i < v_size; ++i) { sum_squared_diff += (v[i] - mean_value) * (v[i] - mean_value); } - double divisor = (double)v.size(); - if (useBesselCorrection && v.size() > 1) { + double divisor = static_cast(v.size()); + if (useBesselCorrection && (v.size() > 1)) { divisor = divisor - 1; } @@ -969,7 +1006,7 @@ int vpRowVector::print(std::ostream &s, unsigned int length, char const *intro) std::ostringstream ossFixed; std::ios_base::fmtflags original_flags = oss.flags(); - // ossFixed <(maxAfter, totalLength - maxBefore); - if (maxAfter == 1) + if (maxAfter == 1) { maxAfter = 0; + } // the following line is useful for debugging // std::cerr <(maxBefore)); s << values[j].substr(0, p).c_str(); if (maxAfter > 0) { s.setf(std::ios::left, std::ios::adjustfield); if (p != std::string::npos) { - s.width((std::streamsize)maxAfter); + s.width(static_cast(maxAfter)); s << values[j].substr(p, maxAfter).c_str(); } else { assert(maxAfter > 1); - s.width((std::streamsize)maxAfter); + s.width( static_cast (maxAfter)); s << ".0"; } } @@ -1039,7 +1078,7 @@ int vpRowVector::print(std::ostream &s, unsigned int length, char const *intro) s.flags(original_flags); // restore s to standard state - return (int)(maxBefore + maxAfter); + return static_cast(maxBefore + maxAfter); } /*! @@ -1061,7 +1100,7 @@ double vpRowVector::sum() const { double sum = 0.0; - for (unsigned int j = 0; j < colNum; j++) { + for (unsigned int j = 0; j < colNum; ++j) { sum += rowPtrs[0][j]; } @@ -1078,7 +1117,7 @@ double vpRowVector::sumSquare() const { double sum_square = 0.0; - for (unsigned int j = 0; j < colNum; j++) { + for (unsigned int j = 0; j < colNum; ++j) { double x = rowPtrs[0][j]; sum_square += x * x; } @@ -1150,14 +1189,15 @@ void vpRowVector::init(const vpRowVector &v, unsigned int c, unsigned int ncols) { unsigned int cncols = c + ncols; - if (cncols > v.getCols()) + if (cncols > v.getCols()) { throw(vpException(vpException::dimensionError, "Bad column dimension (%d > %d) used to initialize vpRowVector", cncols, v.getCols())); + } resize(ncols); if (this->rowPtrs == nullptr) { // Fix coverity scan: explicit null dereferenced return; // Noting to do } - for (unsigned int i = 0; i < ncols; i++) { + for (unsigned int i = 0; i < ncols; ++i) { (*this)[i] = v[i + c]; } } @@ -1196,14 +1236,15 @@ std::ostream &vpRowVector::cppPrint(std::ostream &os, const std::string &matrixN { os << "vpRowVector " << matrixName << " (" << this->getCols() << "); " << std::endl; - for (unsigned int j = 0; j < this->getCols(); ++j) { + unsigned int this_cols = this->getCols(); + for (unsigned int j = 0; j < this_cols; ++j) { if (!octet) { os << matrixName << "[" << j << "] = " << (*this)[j] << "; " << std::endl; } else { for (unsigned int k = 0; k < sizeof(double); ++k) { os << "((unsigned char*)&(" << matrixName << "[" << j << "]) )[" << k << "] = 0x" << std::hex - << (unsigned int)((unsigned char *)&((*this)[j]))[k] << "; " << std::endl; + << static_cast(((unsigned char *)&((*this)[j]))[k]) << "; " << std::endl; } } } @@ -1237,10 +1278,12 @@ int main() */ std::ostream &vpRowVector::csvPrint(std::ostream &os) const { - for (unsigned int j = 0; j < this->getCols(); ++j) { + unsigned int this_cols = this->getCols(); + for (unsigned int j = 0; j < this_cols; ++j) { os << (*this)[j]; - if (!(j == (this->getCols() - 1))) + if (!(j == (this->getCols() - 1))) { os << ", "; + } } os << std::endl; return os; @@ -1273,7 +1316,8 @@ std::ostream &vpRowVector::maplePrint(std::ostream &os) const { os << "([ " << std::endl; os << "["; - for (unsigned int j = 0; j < this->getCols(); ++j) { + unsigned int this_cols = this->getCols(); + for (unsigned int j = 0; j < this_cols; ++j) { os << (*this)[j] << ", "; } os << "]," << std::endl; @@ -1314,7 +1358,9 @@ r = std::ostream &vpRowVector::matlabPrint(std::ostream &os) const { os << "[ "; - for (unsigned int j = 0; j < this->getCols(); ++j) { + + unsigned int this_cols = this->getCols(); + for (unsigned int j = 0; j < this_cols; ++j) { os << (*this)[j] << ", "; } os << "]" << std::endl; diff --git a/modules/core/src/math/matrix/vpSubColVector.cpp b/modules/core/src/math/matrix/vpSubColVector.cpp index 32873ff54b..bdebd53e90 100644 --- a/modules/core/src/math/matrix/vpSubColVector.cpp +++ b/modules/core/src/math/matrix/vpSubColVector.cpp @@ -70,7 +70,7 @@ void vpSubColVector::init(vpColVector &v, const unsigned int &offset, const unsi "empty parent column vector")); } - if (offset + nrows <= v.getRows()) { + if ((offset + nrows) <= v.getRows()) { data = v.data + offset; rowNum = nrows; @@ -84,8 +84,9 @@ void vpSubColVector::init(vpColVector &v, const unsigned int &offset, const unsi } rowPtrs = (double **)malloc(m_parent->getRows() * sizeof(double *)); - for (unsigned int i = 0; i < nrows; i++) - rowPtrs[i] = v.data + i + offset; + for (unsigned int i = 0; i < nrows; ++i) { + rowPtrs[i] = v.data + (i + offset); + } dsize = rowNum; } @@ -130,8 +131,9 @@ vpSubColVector &vpSubColVector::operator=(const vpSubColVector &B) rowNum, B.getRows())); } m_pRowNum = B.m_pRowNum; - for (unsigned int i = 0; i < rowNum; i++) + for (unsigned int i = 0; i < rowNum; ++i) { data[i] = B[i]; + } return *this; } @@ -150,8 +152,9 @@ vpSubColVector &vpSubColVector::operator=(const vpColVector &B) rowNum, B.getRows())); } - for (unsigned int i = 0; i < rowNum; i++) + for (unsigned int i = 0; i < rowNum; ++i) { data[i] = B[i]; + } return *this; } @@ -169,8 +172,9 @@ vpSubColVector &vpSubColVector::operator=(const vpMatrix &B) rowNum, B.getRows(), B.getCols())); } - for (unsigned int i = 0; i < rowNum; i++) + for (unsigned int i = 0; i < rowNum; ++i) { data[i] = B[i][1]; + } return *this; } @@ -181,8 +185,9 @@ vpSubColVector &vpSubColVector::operator=(const vpMatrix &B) */ vpSubColVector &vpSubColVector::operator=(const double &x) { - for (unsigned int i = 0; i < rowNum; i++) + for (unsigned int i = 0; i < rowNum; ++i) { data[i] = x; + } return *this; } diff --git a/modules/core/src/math/matrix/vpSubMatrix.cpp b/modules/core/src/math/matrix/vpSubMatrix.cpp index 517450f790..6deeaf6467 100644 --- a/modules/core/src/math/matrix/vpSubMatrix.cpp +++ b/modules/core/src/math/matrix/vpSubMatrix.cpp @@ -75,7 +75,7 @@ void vpSubMatrix::init(vpMatrix &m, const unsigned int &row_offset, const unsign throw(vpMatrixException(vpMatrixException::subMatrixError, "SubMatrix parent matrix is not allocated")); } - if (row_offset + nrows <= m.getRows() && col_offset + ncols <= m.getCols()) { + if ((row_offset + nrows <= m.getRows()) && ((col_offset + ncols) <= m.getCols())) { data = m.data; parent = &m; rowNum = nrows; @@ -83,12 +83,14 @@ void vpSubMatrix::init(vpMatrix &m, const unsigned int &row_offset, const unsign pRowNum = m.getRows(); pColNum = m.getCols(); - if (rowPtrs) + if (rowPtrs) { free(rowPtrs); + } rowPtrs = (double **)malloc(nrows * sizeof(double *)); - for (unsigned int r = 0; r < nrows; r++) - rowPtrs[r] = m.data + col_offset + (r + row_offset) * pColNum; + for (unsigned int r = 0; r < nrows; ++r) { + rowPtrs[r] = m.data + col_offset + ((r + row_offset) * pColNum); + } dsize = pRowNum * pColNum; } @@ -108,7 +110,7 @@ void vpSubMatrix::checkParentStatus() const throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError, "vpSubMatrix parent vpMatrix has been destroyed")); } - if (pRowNum != parent->getRows() || pColNum != parent->getCols()) { + if ((pRowNum != parent->getRows()) || (pColNum != parent->getCols())) { throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError, "vpSubMatrix size of parent vpMatrix has been changed")); } @@ -126,9 +128,10 @@ vpSubMatrix &vpSubMatrix::operator=(const vpMatrix &B) "vpSubMatrix mismatch in operator vpSubMatrix=vpMatrix")); } - for (unsigned int i = 0; i < rowNum; i++) { - for (unsigned int j = 0; j < colNum; j++) + for (unsigned int i = 0; i < rowNum; ++i) { + for (unsigned int j = 0; j < colNum; ++j) { rowPtrs[i][j] = B[i][j]; + } } return *this; @@ -152,9 +155,10 @@ vpSubMatrix &vpSubMatrix::operator=(const vpSubMatrix &B) double **BrowPtrs = B.rowPtrs; - for (unsigned int i = 0; i < rowNum; i++) { - for (unsigned int j = 0; j < colNum; j++) + for (unsigned int i = 0; i < rowNum; ++i) { + for (unsigned int j = 0; j < colNum; ++j) { rowPtrs[i][j] = BrowPtrs[i][j]; + } } return *this; @@ -166,9 +170,10 @@ vpSubMatrix &vpSubMatrix::operator=(const vpSubMatrix &B) */ vpSubMatrix &vpSubMatrix::operator=(const double &x) { - for (unsigned int i = 0; i < rowNum; i++) { - for (unsigned int j = 0; j < colNum; j++) + for (unsigned int i = 0; i < rowNum; ++i) { + for (unsigned int j = 0; j < colNum; ++j) { rowPtrs[i][j] = x; + } } return *this; diff --git a/modules/core/src/math/matrix/vpSubRowVector.cpp b/modules/core/src/math/matrix/vpSubRowVector.cpp index 6f7b10bc8e..f826982e6d 100644 --- a/modules/core/src/math/matrix/vpSubRowVector.cpp +++ b/modules/core/src/math/matrix/vpSubRowVector.cpp @@ -64,7 +64,7 @@ void vpSubRowVector::init(vpRowVector &v, const unsigned int &offset, const unsi "vector from an empty parent row vector")); } - if (offset + ncols <= v.getCols()) { + if ((offset + ncols) <= v.getCols()) { data = v.data + offset; rowNum = 1; @@ -73,12 +73,14 @@ void vpSubRowVector::init(vpRowVector &v, const unsigned int &offset, const unsi m_pColNum = v.getCols(); m_parent = &v; - if (rowPtrs) + if (rowPtrs) { free(rowPtrs); + } rowPtrs = (double **)malloc(1 * sizeof(double *)); - for (unsigned int i = 0; i < 1; i++) + for (unsigned int i = 0; i < 1; ++i) { rowPtrs[i] = v.data + i + offset; + } dsize = colNum; } @@ -122,8 +124,9 @@ vpSubRowVector &vpSubRowVector::operator=(const vpSubRowVector &B) } m_pColNum = B.m_pColNum; m_parent = B.m_parent; - for (unsigned int i = 0; i < rowNum; i++) + for (unsigned int i = 0; i < rowNum; ++i) { data[i] = B[i]; + } return *this; } @@ -141,8 +144,9 @@ vpSubRowVector &vpSubRowVector::operator=(const vpRowVector &B) colNum, B.getCols())); } - for (unsigned int i = 0; i < rowNum; i++) + for (unsigned int i = 0; i < rowNum; ++i) { data[i] = B[i]; + } return *this; } @@ -160,8 +164,9 @@ vpSubRowVector &vpSubRowVector::operator=(const vpMatrix &B) colNum, B.getRows(), B.getCols())); } - for (unsigned int i = 0; i < rowNum; i++) + for (unsigned int i = 0; i < rowNum; ++i) { data[i] = B[i][1]; + } return *this; } @@ -171,7 +176,8 @@ vpSubRowVector &vpSubRowVector::operator=(const vpMatrix &B) */ vpSubRowVector &vpSubRowVector::operator=(const double &x) { - for (unsigned int i = 0; i < rowNum; i++) + for (unsigned int i = 0; i < rowNum; ++i) { data[i] = x; + } return *this; } diff --git a/modules/core/src/math/misc/vpMath.cpp b/modules/core/src/math/misc/vpMath.cpp index c7f9b1e2c7..2e6683d478 100644 --- a/modules/core/src/math/misc/vpMath.cpp +++ b/modules/core/src/math/misc/vpMath.cpp @@ -215,7 +215,7 @@ bool vpMath::isFinite(float value) bool vpMath::isNumber(const std::string &str) { size_t str_size = str.size(); - for (size_t i = 0; i < str_size; i++) { + for (size_t i = 0; i < str_size; ++i) { if (isdigit(str[i]) == false) { return false; } @@ -396,7 +396,7 @@ double vpMath::lineFitting(const std::vector &imPts, double &a, do double x_mean = 0, y_mean = 0; size_t imPts_size = imPts.size(); - for (size_t i = 0; i < imPts_size; i++) { + for (size_t i = 0; i < imPts_size; ++i) { const vpImagePoint &imPt = imPts[i]; x_mean += imPt.get_u(); y_mean += imPt.get_v(); @@ -406,7 +406,7 @@ double vpMath::lineFitting(const std::vector &imPts, double &a, do vpMatrix AtA(2, 2, 0.0); imPts_size = imPts.size(); - for (size_t i = 0; i < imPts_size; i++) { + for (size_t i = 0; i < imPts_size; ++i) { const vpImagePoint &imPt = imPts[i]; AtA[0][0] += (imPt.get_u() - x_mean) * (imPt.get_u() - x_mean); AtA[0][1] += (imPt.get_u() - x_mean) * (imPt.get_v() - y_mean); @@ -424,7 +424,7 @@ double vpMath::lineFitting(const std::vector &imPts, double &a, do double error = 0; imPts_size = imPts.size(); - for (size_t i = 0; i < imPts_size; i++) { + for (size_t i = 0; i < imPts_size; ++i) { double x0 = imPts[i].get_u(); double y0 = imPts[i].get_v(); @@ -598,11 +598,11 @@ std::vector > vpMath::computeRegularPointsOnSphere(uns lonlat_vec.reserve(static_cast(std::sqrt(static_cast(maxPoints)))); #endif - for (int m = 0; m < m_theta; m++) { + for (int m = 0; m < m_theta; ++m) { double theta = (M_PI * (m + 0.5)) / m_theta; int m_phi = static_cast(round((2.0 * M_PI * sin(theta)) / d_phi)); - for (int n = 0; n < m_phi; n++) { + for (int n = 0; n < m_phi; ++n) { double phi = (2.0 * M_PI * n) / m_phi; double lon = phi; double lat = M_PI_2 - theta; @@ -638,7 +638,7 @@ std::vector vpMath::getLocalTangentPlaneTransformations(con std::vector ecef_M_local_vec; ecef_M_local_vec.reserve(lonlatVec.size()); size_t lonlatVec_size = lonlatVec.size(); - for (size_t i = 0; i < lonlatVec_size; i++) { + for (size_t i = 0; i < lonlatVec_size; ++i) { double lonDeg = lonlatVec[i].first; double latDeg = lonlatVec[i].second; @@ -707,7 +707,7 @@ vpColVector vpMath::deg(const vpRotationVector &r) } vpColVector r_deg(r.size()); unsigned int r_size = r.size(); - for (unsigned int i = 0; i < r_size; i++) { + for (unsigned int i = 0; i < r_size; ++i) { r_deg[i] = vpMath::deg(r[i]); } return r_deg; @@ -723,7 +723,7 @@ vpColVector vpMath::deg(const vpColVector &r) { vpColVector r_deg(r.size()); unsigned int r_size = r.size(); - for (unsigned int i = 0; i < r_size; i++) { + for (unsigned int i = 0; i < r_size; ++i) { r_deg[i] = vpMath::deg(r[i]); } return r_deg; @@ -739,7 +739,7 @@ vpColVector vpMath::rad(const vpColVector &r) { vpColVector r_rad(r.size()); unsigned int r_size = r.size(); - for (unsigned int i = 0; i < r_size; i++) { + for (unsigned int i = 0; i < r_size; ++i) { r_rad[i] = vpMath::rad(r[i]); } return r_rad; diff --git a/modules/core/src/math/random-generator/vpUniRand.cpp b/modules/core/src/math/random-generator/vpUniRand.cpp index 2fdd03351b..27841d8de4 100644 --- a/modules/core/src/math/random-generator/vpUniRand.cpp +++ b/modules/core/src/math/random-generator/vpUniRand.cpp @@ -170,14 +170,14 @@ int vpUniRand::uniform(int a, int b) \param a : lower inclusive boundary of the returned random number. \param b : upper non-inclusive boundary of the returned random number. */ -float vpUniRand::uniform(float a, float b) { return next() * m_maxInvFlt * (b - a) + a; } +float vpUniRand::uniform(float a, float b) { return (next() * m_maxInvFlt * (b - a)) + a; } /*! Generates a pseudorandom uniformly distributed double number between [a, b) range. \param a : lower inclusive boundary of the returned random number. \param b : upper non-inclusive boundary of the returned random number. */ -double vpUniRand::uniform(double a, double b) { return next() * m_maxInvDbl * (b - a) + a; } +double vpUniRand::uniform(double a, double b) { return (next() * m_maxInvDbl * (b - a)) + a; } /*! Initialize the random number generator. diff --git a/modules/core/src/math/robust/vpRobust.cpp b/modules/core/src/math/robust/vpRobust.cpp index 7a87b9df87..abbadffc39 100644 --- a/modules/core/src/math/robust/vpRobust.cpp +++ b/modules/core/src/math/robust/vpRobust.cpp @@ -145,14 +145,14 @@ void vpRobust::MEstimator(const vpRobustEstimatorType method, const vpColVector m_sorted_residues = residues; - unsigned int ind_med = (unsigned int)(ceil(n_data / 2.0)) - 1; + unsigned int ind_med = static_cast(ceil(n_data / 2.0)) - 1; // Calculate median med = select(m_sorted_residues, 0, n_data - 1, ind_med); - // residualMedian = med ; + // --comment: residualMedian = med // Normalize residues - for (unsigned int i = 0; i < n_data; i++) { + for (unsigned int i = 0; i < n_data; ++i) { m_normres[i] = (fabs(residues[i] - med)); m_sorted_normres[i] = (fabs(m_sorted_residues[i] - med)); } @@ -181,6 +181,9 @@ void vpRobust::MEstimator(const vpRobustEstimatorType method, const vpColVector psiHuber(m_mad, m_normres, weights); break; } + default: + // TODO + std::cout << "MEstimator: method not recognised - id = " << method << std::endl; } } @@ -198,7 +201,7 @@ void vpRobust::psiTukey(double sig, const vpColVector &x, vpColVector &weights) double C = sig * 4.6851; // Here we consider that sig cannot be equal to 0 - for (unsigned int i = 0; i < n_data; i++) { + for (unsigned int i = 0; i < n_data; ++i) { double xi = x[i] / C; xi *= xi; @@ -225,12 +228,14 @@ void vpRobust::psiHuber(double sig, const vpColVector &x, vpColVector &weights) double C = sig * 1.2107; unsigned int n_data = x.getRows(); - for (unsigned int i = 0; i < n_data; i++) { + for (unsigned int i = 0; i < n_data; ++i) { double xi = x[i] / C; - if (fabs(xi) > 1.) + if (fabs(xi) > 1.) { weights[i] = std::fabs(1. / xi); - else + } + else { weights[i] = 1; + } } } @@ -248,8 +253,8 @@ void vpRobust::psiCauchy(double sig, const vpColVector &x, vpColVector &weights) double C = sig * 2.3849; // Calculate Cauchy's equation - for (unsigned int i = 0; i < n_data; i++) { - weights[i] = 1. / (1. + vpMath::sqr(x[i] / (C))); + for (unsigned int i = 0; i < n_data; ++i) { + weights[i] = 1. / (1. + vpMath::sqr(x[i] / C)); } } @@ -290,10 +295,12 @@ double vpRobust::select(vpColVector &a, int l, int r, int k) { while (r > l) { int i = partition(a, l, r); - if (i >= k) + if (i >= k) { r = i - 1; - if (i <= k) + } + if (i <= k) { l = i + 1; + } } return a[k]; } @@ -380,7 +387,7 @@ double vpRobust::computeNormalizedMedian(vpColVector &all_normres, const vpColVe no_null_weight_residues.resize(n_data); unsigned int index = 0; - for (unsigned int j = 0; j < n_data; j++) { + for (unsigned int j = 0; j < n_data; ++j) { // if(weights[j]!=0) if (std::fabs(weights[j]) > std::numeric_limits::epsilon()) { no_null_weight_residues[index] = residues[j]; @@ -399,11 +406,11 @@ double vpRobust::computeNormalizedMedian(vpColVector &all_normres, const vpColVe med = select(m_sorted_residues, 0, n_data - 1, ind_med); // Normalize residues - for (unsigned int i = 0; i < n_all_data; i++) { + for (unsigned int i = 0; i < n_all_data; ++i) { all_normres[i] = (fabs(all_residues[i] - med)); } - for (unsigned int i = 0; i < n_data; i++) { + for (unsigned int i = 0; i < n_data; ++i) { m_sorted_normres[i] = (fabs(m_sorted_residues[i] - med)); } // MAD calculated only on first iteration @@ -431,7 +438,7 @@ vpColVector vpRobust::simultMEstimator(vpColVector &residues) med = select(residues, 0, n_data - 1, ind_med /*(int)n_data/2*/); // Normalize residues - for (unsigned int i = 0; i < n_data; i++) + for (unsigned int i = 0; i < n_data; ++i) norm_res[i] = (fabs(residues[i] - med)); // Check for various methods. @@ -468,7 +475,7 @@ double vpRobust::simultscale(const vpColVector &x) /* long */ double Expectation = 0; /* long */ double Sum_chi = 0; - for (unsigned int i = 0; i < n; i++) { + for (unsigned int i = 0; i < n; ++i) { double chiTmp = simult_chi_huber(x[i]); #if defined(VISP_HAVE_FUNC_STD_ERFC) @@ -632,7 +639,7 @@ void vpRobust::gser(double *gamser, double a, double x, double *gln) double ap = a; double sum = 1.0 / a; double del = sum; - for (int n = 1; n <= vpITMAX; n++) { + for (int n = 1; n <= vpITMAX; ++n) { ap += 1.0; del *= x / ap; sum += del; @@ -653,8 +660,8 @@ void vpRobust::gcf(double *gammcf, double a, double x, double *gln) *gln = gammln(a); a1 = x; - for (int n = 1; n <= vpITMAX; n++) { - double an = (double)n; + for (int n = 1; n <= vpITMAX; ++n) { + double an = static_cast(n); double ana = an - a; a0 = (a1 + a0 * ana) * fac; b0 = (b1 + b0 * ana) * fac; @@ -684,7 +691,7 @@ double vpRobust::gammln(double xx) tmp = x + 5.5; tmp -= (x + 0.5) * log(tmp); ser = 1.0; - for (int j = 0; j <= 5; j++) { + for (int j = 0; j <= 5; ++j) { x += 1.0; ser += cof[j] / x; } diff --git a/modules/core/src/math/transformation/vpExponentialMap.cpp b/modules/core/src/math/transformation/vpExponentialMap.cpp index 003abc1ca4..7706cdd132 100644 --- a/modules/core/src/math/transformation/vpExponentialMap.cpp +++ b/modules/core/src/math/transformation/vpExponentialMap.cpp @@ -94,21 +94,24 @@ vpHomogeneousMatrix vpExponentialMap::direct(const vpColVector &v, const double u[2] = v_dt[5]; rd.buildFrom(u); - theta = sqrt(u[0] * u[0] + u[1] * u[1] + u[2] * u[2]); + theta = sqrt((u[0] * u[0]) + (u[1] * u[1]) + (u[2] * u[2])); si = sin(theta); co = cos(theta); sinc = vpMath::sinc(si, theta); mcosc = vpMath::mcosc(co, theta); msinc = vpMath::msinc(si, theta); - dt[0] = v_dt[0] * (sinc + u[0] * u[0] * msinc) + v_dt[1] * (u[0] * u[1] * msinc - u[2] * mcosc) + - v_dt[2] * (u[0] * u[2] * msinc + u[1] * mcosc); + dt[0] = ((v_dt[0] * (sinc + (u[0] * u[0] * msinc))) + + (v_dt[1] * ((u[0] * u[1] * msinc) - (u[2] * mcosc)))) + + (v_dt[2] * ((u[0] * u[2] * msinc) + (u[1] * mcosc))); - dt[1] = v_dt[0] * (u[0] * u[1] * msinc + u[2] * mcosc) + v_dt[1] * (sinc + u[1] * u[1] * msinc) + - v_dt[2] * (u[1] * u[2] * msinc - u[0] * mcosc); + dt[1] = ((v_dt[0] * ((u[0] * u[1] * msinc) + (u[2] * mcosc))) + + (v_dt[1] * (sinc + (u[1] * u[1] * msinc)))) + + (v_dt[2] * ((u[1] * u[2] * msinc) - (u[0] * mcosc))); - dt[2] = v_dt[0] * (u[0] * u[2] * msinc - u[1] * mcosc) + v_dt[1] * (u[1] * u[2] * msinc + u[0] * mcosc) + - v_dt[2] * (sinc + u[2] * u[2] * msinc); + dt[2] = ((v_dt[0] * ((u[0] * u[2] * msinc) - (u[1] * mcosc))) + + (v_dt[1] * ((u[1] * u[2] * msinc) + (u[0] * mcosc)))) + + (v_dt[2] * (sinc + (u[2] * u[2] * msinc))); vpHomogeneousMatrix Delta; Delta.insert(rd); @@ -120,42 +123,42 @@ vpHomogeneousMatrix vpExponentialMap::direct(const vpColVector &v, const double unsigned int i, j; double s; - // double u[3]; - // vpRotationMatrix rd ; - // vpTranslationVector dt ; - s = sqrt(v_dt[3] * v_dt[3] + v_dt[4] * v_dt[4] + v_dt[5] * v_dt[5]); + s = sqrt((v_dt[3] * v_dt[3]) + (v_dt[4] * v_dt[4]) + (v_dt[5] * v_dt[5])); if (s > 1.e-15) { - for (i = 0; i < 3; i++) + for (i = 0; i < 3; ++i) { u[i] = v_dt[3 + i] / s; + } double sinu = sin(s); double cosi = cos(s); double mcosi = 1 - cosi; - rd[0][0] = cosi + mcosi * u[0] * u[0]; - rd[0][1] = -sinu * u[2] + mcosi * u[0] * u[1]; - rd[0][2] = sinu * u[1] + mcosi * u[0] * u[2]; - rd[1][0] = sinu * u[2] + mcosi * u[1] * u[0]; - rd[1][1] = cosi + mcosi * u[1] * u[1]; - rd[1][2] = -sinu * u[0] + mcosi * u[1] * u[2]; - rd[2][0] = -sinu * u[1] + mcosi * u[2] * u[0]; - rd[2][1] = sinu * u[0] + mcosi * u[2] * u[1]; - rd[2][2] = cosi + mcosi * u[2] * u[2]; - - dt[0] = v_dt[0] * (sinu / s + u[0] * u[0] * (1 - sinu / s)) + - v_dt[1] * (u[0] * u[1] * (1 - sinu / s) - u[2] * mcosi / s) + - v_dt[2] * (u[0] * u[2] * (1 - sinu / s) + u[1] * mcosi / s); - - dt[1] = v_dt[0] * (u[0] * u[1] * (1 - sinu / s) + u[2] * mcosi / s) + - v_dt[1] * (sinu / s + u[1] * u[1] * (1 - sinu / s)) + - v_dt[2] * (u[1] * u[2] * (1 - sinu / s) - u[0] * mcosi / s); - - dt[2] = v_dt[0] * (u[0] * u[2] * (1 - sinu / s) - u[1] * mcosi / s) + - v_dt[1] * (u[1] * u[2] * (1 - sinu / s) + u[0] * mcosi / s) + - v_dt[2] * (sinu / s + u[2] * u[2] * (1 - sinu / s)); - } else { - for (i = 0; i < 3; i++) { - for (j = 0; j < 3; j++) + rd[0][0] = cosi + (mcosi * u[0] * u[0]); + rd[0][1] = (-sinu * u[2]) + (mcosi * u[0] * u[1]); + rd[0][2] = (sinu * u[1]) + (mcosi * u[0] * u[2]); + rd[1][0] = (sinu * u[2]) + (mcosi * u[1] * u[0]); + rd[1][1] = cosi + (mcosi * u[1] * u[1]); + rd[1][2] = (-sinu * u[0]) + (mcosi * u[1] * u[2]); + rd[2][0] = (-sinu * u[1]) + (mcosi * u[2] * u[0]); + rd[2][1] = (sinu * u[0]) + (mcosi * u[2] * u[1]); + rd[2][2] = cosi + (mcosi * u[2] * u[2]); + + dt[0] = (v_dt[0] * ((sinu / s) + (u[0] * u[0] * (1 - (sinu / s))))) + + (v_dt[1] * ((u[0] * u[1] * (1 - (sinu / s))) - ((u[2] * mcosi) / s))) + + (v_dt[2] * ((u[0] * u[2] * (1 - (sinu / s))) + ((u[1] * mcosi) / s))); + + dt[1] = (v_dt[0] * ((u[0] * u[1] * (1 - (sinu / s))) + ((u[2] * mcosi) / s))) + + (v_dt[1] * ((sinu / s) + (u[1] * u[1] * (1 - (sinu / s))))) + + (v_dt[2] * ((u[1] * u[2] * (1 - (sinu / s))) - ((u[0] * mcosi) / s))); + + dt[2] = (v_dt[0] * ((u[0] * u[2] * (1 - (sinu / s))) - ((u[1] * mcosi) / s))) + + (v_dt[1] * ((u[1] * u[2] * (1 - (sinu / s))) + ((u[0] * mcosi) / s))) + + (v_dt[2] * ((sinu / s) + (u[2] * u[2] * (1 - (sinu / s))))); + } + else { + for (i = 0; i < 3; ++i) { + for (j = 0; j < 3; ++j) { rd[i][j] = 0.0; + } rd[i][i] = 1.0; dt[i] = v_dt[i]; } @@ -168,10 +171,12 @@ vpHomogeneousMatrix vpExponentialMap::direct(const vpColVector &v, const double Delta_old.insert(dt); int pb = 0; - for (i = 0; i < 4; i++) { - for (j = 0; j < 4; j++) - if (fabs(Delta[i][j] - Delta_old[i][j]) > 1.e-5) + for (i = 0; i < 4; ++i) { + for (j = 0; j < 4; ++j) { + if (fabs(Delta[i][j] - Delta_old[i][j]) > 1.e-5) { pb = 1; + } + } } if (pb == 1) { printf("pb vpHomogeneousMatrix::expMap\n"); @@ -228,10 +233,11 @@ vpColVector vpExponentialMap::inverse(const vpHomogeneousMatrix &M, const double M.extract(Rd); u.buildFrom(Rd); - for (i = 0; i < 3; i++) + for (i = 0; i < 3; ++i) { v[3 + i] = u[i]; + } - theta = sqrt(u[0] * u[0] + u[1] * u[1] + u[2] * u[2]); + theta = sqrt((u[0] * u[0]) + (u[1] * u[1]) + (u[2] * u[2])); si = sin(theta); co = cos(theta); sinc = vpMath::sinc(si, theta); @@ -242,32 +248,33 @@ vpColVector vpExponentialMap::inverse(const vpHomogeneousMatrix &M, const double // the Rodrigues formula : sinc I + (1-sinc)/t^2 VV^T + (1-cos)/t^2 [V]_X // with V = t.U - a[0][0] = sinc + u[0] * u[0] * msinc; - a[0][1] = u[0] * u[1] * msinc - u[2] * mcosc; - a[0][2] = u[0] * u[2] * msinc + u[1] * mcosc; + a[0][0] = sinc + (u[0] * u[0] * msinc); + a[0][1] = (u[0] * u[1] * msinc) - (u[2] * mcosc); + a[0][2] = (u[0] * u[2] * msinc) + (u[1] * mcosc); - a[1][0] = u[0] * u[1] * msinc + u[2] * mcosc; - a[1][1] = sinc + u[1] * u[1] * msinc; - a[1][2] = u[1] * u[2] * msinc - u[0] * mcosc; + a[1][0] = (u[0] * u[1] * msinc) + (u[2] * mcosc); + a[1][1] = sinc + (u[1] * u[1] * msinc); + a[1][2] = (u[1] * u[2] * msinc) - (u[0] * mcosc); - a[2][0] = u[0] * u[2] * msinc - u[1] * mcosc; - a[2][1] = u[1] * u[2] * msinc + u[0] * mcosc; - a[2][2] = sinc + u[2] * u[2] * msinc; + a[2][0] = (u[0] * u[2] * msinc) - (u[1] * mcosc); + a[2][1] = (u[1] * u[2] * msinc) + (u[0] * mcosc); + a[2][2] = sinc + (u[2] * u[2] * msinc); - det = a[0][0] * a[1][1] * a[2][2] + a[1][0] * a[2][1] * a[0][2] + a[0][1] * a[1][2] * a[2][0] - - a[2][0] * a[1][1] * a[0][2] - a[1][0] * a[0][1] * a[2][2] - a[0][0] * a[2][1] * a[1][2]; + det = (((((a[0][0] * a[1][1] * a[2][2]) + (a[1][0] * a[2][1] * a[0][2])) + (a[0][1] * a[1][2] * a[2][0])) - + (a[2][0] * a[1][1] * a[0][2])) - (a[1][0] * a[0][1] * a[2][2])) - (a[0][0] * a[2][1] * a[1][2]); if (fabs(det) > 1.e-5) { - v[0] = (M[0][3] * a[1][1] * a[2][2] + M[1][3] * a[2][1] * a[0][2] + M[2][3] * a[0][1] * a[1][2] - - M[2][3] * a[1][1] * a[0][2] - M[1][3] * a[0][1] * a[2][2] - M[0][3] * a[2][1] * a[1][2]) / - det; - v[1] = (a[0][0] * M[1][3] * a[2][2] + a[1][0] * M[2][3] * a[0][2] + M[0][3] * a[1][2] * a[2][0] - - a[2][0] * M[1][3] * a[0][2] - a[1][0] * M[0][3] * a[2][2] - a[0][0] * M[2][3] * a[1][2]) / - det; - v[2] = (a[0][0] * a[1][1] * M[2][3] + a[1][0] * a[2][1] * M[0][3] + a[0][1] * M[1][3] * a[2][0] - - a[2][0] * a[1][1] * M[0][3] - a[1][0] * a[0][1] * M[2][3] - a[0][0] * a[2][1] * M[1][3]) / - det; - } else { + v[0] = ((((((M[0][3] * a[1][1] * a[2][2]) + (M[1][3] * a[2][1] * a[0][2])) + (M[2][3] * a[0][1] * a[1][2])) - + (M[2][3] * a[1][1] * a[0][2])) - (M[1][3] * a[0][1] * a[2][2])) - (M[0][3] * a[2][1] * a[1][2])) / + det; + v[1] = ((((((a[0][0] * M[1][3] * a[2][2]) + (a[1][0] * M[2][3] * a[0][2])) + (M[0][3] * a[1][2] * a[2][0])) - + (a[2][0] * M[1][3] * a[0][2])) - (a[1][0] * M[0][3] * a[2][2])) - (a[0][0] * M[2][3] * a[1][2])) / + det; + v[2] = ((((((a[0][0] * a[1][1] * M[2][3]) + (a[1][0] * a[2][1] * M[0][3])) + (a[0][1] * M[1][3] * a[2][0])) - + (a[2][0] * a[1][1] * M[0][3])) - (a[1][0] * a[0][1] * M[2][3])) - (a[0][0] * a[2][1] * M[1][3])) / + det; + } + else { v[0] = M[0][3]; v[1] = M[1][3]; v[2] = M[2][3]; @@ -276,5 +283,5 @@ vpColVector vpExponentialMap::inverse(const vpHomogeneousMatrix &M, const double // Apply the sampling time to the computed velocity v /= delta_t; - return (v); + return v; } diff --git a/modules/core/src/math/transformation/vpForceTwistMatrix.cpp b/modules/core/src/math/transformation/vpForceTwistMatrix.cpp index db6fb4a6b6..5637928b9a 100644 --- a/modules/core/src/math/transformation/vpForceTwistMatrix.cpp +++ b/modules/core/src/math/transformation/vpForceTwistMatrix.cpp @@ -56,8 +56,8 @@ */ vpForceTwistMatrix &vpForceTwistMatrix::operator=(const vpForceTwistMatrix &M) { - for (int i = 0; i < 6; i++) { - for (int j = 0; j < 6; j++) { + for (int i = 0; i < 6; ++i) { + for (int j = 0; j < 6; ++j) { rowPtrs[i][j] = M.rowPtrs[i][j]; } } @@ -70,12 +70,14 @@ vpForceTwistMatrix &vpForceTwistMatrix::operator=(const vpForceTwistMatrix &M) */ void vpForceTwistMatrix::eye() { - for (unsigned int i = 0; i < 6; i++) { - for (unsigned int j = 0; j < 6; j++) { - if (i == j) + for (unsigned int i = 0; i < 6; ++i) { + for (unsigned int j = 0; j < 6; ++j) { + if (i == j) { (*this)[i][j] = 1.0; - else + } + else { (*this)[i][j] = 0.0; + } } } } @@ -125,10 +127,12 @@ vpForceTwistMatrix::vpForceTwistMatrix(const vpForceTwistMatrix &F) : vpArray2D< */ vpForceTwistMatrix::vpForceTwistMatrix(const vpHomogeneousMatrix &M, bool full) : vpArray2D(6, 6) { - if (full) + if (full) { buildFrom(M); - else + } + else { buildFrom(M.getRotationMatrix()); + } } /*! @@ -269,11 +273,12 @@ vpForceTwistMatrix vpForceTwistMatrix::operator*(const vpForceTwistMatrix &F) co { vpForceTwistMatrix Fout; - for (unsigned int i = 0; i < 6; i++) { - for (unsigned int j = 0; j < 6; j++) { + for (unsigned int i = 0; i < 6; ++i) { + for (unsigned int j = 0; j < 6; ++j) { double s = 0; - for (unsigned int k = 0; k < 6; k++) + for (unsigned int k = 0; k < 6; ++k) { s += rowPtrs[i][k] * F.rowPtrs[k][j]; + } Fout[i][j] = s; } } @@ -295,12 +300,14 @@ vpMatrix vpForceTwistMatrix::operator*(const vpMatrix &M) const "Cannot multiply (6x6) force/torque twist matrix by a (%dx%d) matrix", M.getRows(), M.getCols())); } + unsigned int m_col = M.getCols(); vpMatrix p(6, M.getCols()); - for (unsigned int i = 0; i < 6; i++) { - for (unsigned int j = 0; j < M.getCols(); j++) { + for (unsigned int i = 0; i < 6; ++i) { + for (unsigned int j = 0; j < m_col; ++j) { double s = 0; - for (unsigned int k = 0; k < 6; k++) + for (unsigned int k = 0; k < 6; ++k) { s += rowPtrs[i][k] * M[k][j]; + } p[i][j] = s; } } @@ -364,8 +371,8 @@ vpColVector vpForceTwistMatrix::operator*(const vpColVector &H) const Hout = 0.0; - for (unsigned int i = 0; i < 6; i++) { - for (unsigned int j = 0; j < 6; j++) { + for (unsigned int i = 0; i < 6; ++i) { + for (unsigned int j = 0; j < 6; ++j) { Hout[i] += rowPtrs[i][j] * H[j]; } } @@ -396,14 +403,14 @@ vpForceTwistMatrix vpForceTwistMatrix::buildFrom(const vpTranslationVector &t, c { vpMatrix skewaR = t.skew(t) * R; - 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) { (*this)[i][j] = R[i][j]; (*this)[i + 3][j + 3] = R[i][j]; (*this)[i + 3][j] = skewaR[i][j]; } } - return (*this); + return *this; } /*! @@ -425,14 +432,14 @@ vpForceTwistMatrix vpForceTwistMatrix::buildFrom(const vpTranslationVector &t, c */ vpForceTwistMatrix vpForceTwistMatrix::buildFrom(const vpRotationMatrix &R) { - 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) { (*this)[i][j] = R[i][j]; (*this)[i + 3][j + 3] = R[i][j]; (*this)[i + 3][j] = 0; } } - return (*this); + return *this; } /*! @@ -458,7 +465,7 @@ vpForceTwistMatrix vpForceTwistMatrix::buildFrom(const vpRotationMatrix &R) vpForceTwistMatrix vpForceTwistMatrix::buildFrom(const vpTranslationVector &tv, const vpThetaUVector &thetau) { buildFrom(tv, vpRotationMatrix(thetau)); - return (*this); + return *this; } /*! @@ -482,7 +489,7 @@ vpForceTwistMatrix vpForceTwistMatrix::buildFrom(const vpTranslationVector &tv, vpForceTwistMatrix vpForceTwistMatrix::buildFrom(const vpThetaUVector &thetau) { buildFrom(vpRotationMatrix(thetau)); - return (*this); + return *this; } /*! @@ -515,12 +522,14 @@ vpForceTwistMatrix vpForceTwistMatrix::buildFrom(const vpThetaUVector &thetau) */ vpForceTwistMatrix vpForceTwistMatrix::buildFrom(const vpHomogeneousMatrix &M, bool full) { - if (full) + if (full) { buildFrom(M.getTranslationVector(), M.getRotationMatrix()); - else + } + else { buildFrom(M.getRotationMatrix()); + } - return (*this); + return *this; } /*! @@ -554,7 +563,7 @@ int vpForceTwistMatrix::print(std::ostream &s, unsigned int length, char const * std::ostringstream ossFixed; std::ios_base::fmtflags original_flags = oss.flags(); - // ossFixed <(maxAfter, totalLength - maxBefore); - if (maxAfter == 1) + if (maxAfter == 1) { maxAfter = 0; + } // the following line is useful for debugging // std::cerr <(maxBefore)); + s << values[(i * n) + j].substr(0, p).c_str(); if (maxAfter > 0) { s.setf(std::ios::left, std::ios::adjustfield); if (p != std::string::npos) { - s.width((std::streamsize)maxAfter); - s << values[i * n + j].substr(p, maxAfter).c_str(); + s.width(static_cast(maxAfter)); + s << values[(i * n) + j].substr(p, maxAfter).c_str(); } else { assert(maxAfter > 1); - s.width((std::streamsize)maxAfter); + s.width(static_cast(maxAfter)); s << ".0"; } } @@ -628,7 +639,7 @@ int vpForceTwistMatrix::print(std::ostream &s, unsigned int length, char const * s.flags(original_flags); // restore s to standard state - return (int)(maxBefore + maxAfter); + return static_cast(maxBefore + maxAfter); } #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) diff --git a/modules/core/src/math/transformation/vpHomogeneousMatrix.cpp b/modules/core/src/math/transformation/vpHomogeneousMatrix.cpp index d2f2329020..8bbb8baf18 100644 --- a/modules/core/src/math/transformation/vpHomogeneousMatrix.cpp +++ b/modules/core/src/math/transformation/vpHomogeneousMatrix.cpp @@ -123,7 +123,7 @@ int main() v[11] = 0.5; // tz std::cout << "v: "; - for(unsigned int i=0; i &li } else if (list.size() == 16) { std::copy(list.begin(), list.end(), data); - for (size_t i = 12; i < 15; i++) { + for (size_t i = 12; i < 15; ++i) { if (std::fabs(data[i]) > std::numeric_limits::epsilon()) { throw(vpException(vpException::fatalError, "Cannot initialize homogeneous matrix. " @@ -263,7 +263,7 @@ int main() v[11] = 0.5; // tz std::cout << "v: "; - for(unsigned int i=0; i &v) { - if (v.size() != 12 && v.size() != 16) { + if ((v.size() != 12) && (v.size() != 16)) { throw(vpException(vpException::dimensionError, "Cannot convert std::vector to vpHomogeneousMatrix")); } - for (unsigned int i = 0; i < 12; i++) - this->data[i] = (double)v[i]; + for (unsigned int i = 0; i < 12; ++i) { + this->data[i] = static_cast(v[i]); + } } /*! @@ -426,7 +427,7 @@ int main() v[11] = 0.5; // tz std::cout << "v: "; - for(unsigned int i=0; i &v) { - if (v.size() != 12 && v.size() != 16) { + if ((v.size() != 12) && (v.size() != 16)) { throw(vpException(vpException::dimensionError, "Cannot convert std::vector to vpHomogeneousMatrix")); } - for (unsigned int i = 0; i < 12; i++) + for (unsigned int i = 0; i < 12; ++i) { this->data[i] = v[i]; + } } /*! @@ -463,8 +465,8 @@ void vpHomogeneousMatrix::buildFrom(const std::vector &v) */ vpHomogeneousMatrix &vpHomogeneousMatrix::operator=(const vpHomogeneousMatrix &M) { - for (int i = 0; i < 4; i++) { - for (int j = 0; j < 4; j++) { + for (int i = 0; i < 4; ++i) { + for (int j = 0; j < 4; ++j) { rowPtrs[i][j] = M.rowPtrs[i][j]; } } @@ -503,7 +505,7 @@ vpHomogeneousMatrix vpHomogeneousMatrix::operator*(const vpHomogeneousMatrix &M) R = R1 * R2; - T = R1 * T2 + T1; + T = (R1 * T2) + T1; p.insert(T); p.insert(R); @@ -553,8 +555,8 @@ vpColVector vpHomogeneousMatrix::operator*(const vpColVector &v) const p = 0.0; - for (unsigned int j = 0; j < 4; j++) { - for (unsigned int i = 0; i < 4; i++) { + for (unsigned int j = 0; j < 4; ++j) { + for (unsigned int i = 0; i < 4; ++i) { p[i] += rowPtrs[i][j] * v[j]; } } @@ -584,14 +586,14 @@ vpPoint vpHomogeneousMatrix::operator*(const vpPoint &bP) const v[2] = bP.get_Z(); v[3] = bP.get_W(); - v1[0] = (*this)[0][0] * v[0] + (*this)[0][1] * v[1] + (*this)[0][2] * v[2] + (*this)[0][3] * v[3]; - v1[1] = (*this)[1][0] * v[0] + (*this)[1][1] * v[1] + (*this)[1][2] * v[2] + (*this)[1][3] * v[3]; - v1[2] = (*this)[2][0] * v[0] + (*this)[2][1] * v[1] + (*this)[2][2] * v[2] + (*this)[2][3] * v[3]; - v1[3] = (*this)[3][0] * v[0] + (*this)[3][1] * v[1] + (*this)[3][2] * v[2] + (*this)[3][3] * v[3]; + v1[0] = ((*this)[0][0] * v[0]) + ((*this)[0][1] * v[1]) + ((*this)[0][2] * v[2]) + ((*this)[0][3] * v[3]); + v1[1] = ((*this)[1][0] * v[0]) + ((*this)[1][1] * v[1]) + ((*this)[1][2] * v[2]) + ((*this)[1][3] * v[3]); + v1[2] = ((*this)[2][0] * v[0]) + ((*this)[2][1] * v[1]) + ((*this)[2][2] * v[2]) + ((*this)[2][3] * v[3]); + v1[3] = ((*this)[3][0] * v[0]) + ((*this)[3][1] * v[1]) + ((*this)[3][2] * v[2]) + ((*this)[3][3] * v[3]); v1 /= v1[3]; - // v1 = M*v ; + // --comment: v1 = M*v aP.set_X(v1[0]); aP.set_Y(v1[1]); aP.set_Z(v1[2]); @@ -618,9 +620,9 @@ vpPoint vpHomogeneousMatrix::operator*(const vpPoint &bP) const vpTranslationVector vpHomogeneousMatrix::operator*(const vpTranslationVector &t) const { vpTranslationVector t_out; - t_out[0] = (*this)[0][0] * t[0] + (*this)[0][1] * t[1] + (*this)[0][2] * t[2] + (*this)[0][3]; - t_out[1] = (*this)[1][0] * t[0] + (*this)[1][1] * t[1] + (*this)[1][2] * t[2] + (*this)[1][3]; - t_out[2] = (*this)[2][0] * t[0] + (*this)[2][1] * t[1] + (*this)[2][2] * t[2] + (*this)[2][3]; + t_out[0] = (((*this)[0][0] * t[0]) + ((*this)[0][1] * t[1]) + ((*this)[0][2] * t[2])) + (*this)[0][3]; + t_out[1] = (((*this)[1][0] * t[0]) + ((*this)[1][1] * t[1]) + ((*this)[1][2] * t[2])) + (*this)[1][3]; + t_out[2] = (((*this)[2][0] * t[0]) + ((*this)[2][1] * t[1]) + ((*this)[2][2] * t[2])) + (*this)[2][3]; return t_out; } @@ -772,7 +774,8 @@ bool vpHomogeneousMatrix::isAnHomogeneousMatrix(double threshold) const */ bool vpHomogeneousMatrix::isValid() const { - for (unsigned int i = 0; i < size(); i++) { + unsigned int l_size = size(); + for (unsigned int i = 0; i < l_size; ++i) { if (vpMath::isNaN(data[i])) { return false; } @@ -786,9 +789,11 @@ bool vpHomogeneousMatrix::isValid() const */ void vpHomogeneousMatrix::extract(vpRotationMatrix &R) const { - 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) { R[i][j] = (*this)[i][j]; + } + } } /*! @@ -825,9 +830,11 @@ void vpHomogeneousMatrix::extract(vpQuaternionVector &q) const */ void vpHomogeneousMatrix::insert(const vpRotationMatrix &R) { - 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) { (*this)[i][j] = R[i][j]; + } + } } /*! @@ -902,10 +909,18 @@ void vpHomogeneousMatrix::eye() (*this)[2][2] = 1; (*this)[3][3] = 1; - (*this)[0][1] = (*this)[0][2] = (*this)[0][3] = 0; - (*this)[1][0] = (*this)[1][2] = (*this)[1][3] = 0; - (*this)[2][0] = (*this)[2][1] = (*this)[2][3] = 0; - (*this)[3][0] = (*this)[3][1] = (*this)[3][2] = 0; + (*this)[0][1] = 0; + (*this)[0][2] = 0; + (*this)[0][3] = 0; + (*this)[1][0] = 0; + (*this)[1][2] = 0; + (*this)[1][3] = 0; + (*this)[2][0] = 0; + (*this)[2][1] = 0; + (*this)[2][3] = 0; + (*this)[3][0] = 0; + (*this)[3][1] = 0; + (*this)[3][2] = 0; } /*! @@ -945,8 +960,8 @@ void vpHomogeneousMatrix::save(const std::string &filename) const void vpHomogeneousMatrix::load(std::ifstream &f) { if (!f.fail()) { - for (unsigned int i = 0; i < 4; i++) { - for (unsigned int j = 0; j < 4; j++) { + for (unsigned int i = 0; i < 4; ++i) { + for (unsigned int j = 0; j < 4; ++j) { f >> (*this)[i][j]; } } @@ -997,8 +1012,9 @@ void vpHomogeneousMatrix::print() const void vpHomogeneousMatrix::convert(std::vector &M) { M.resize(12); - for (unsigned int i = 0; i < 12; i++) - M[i] = (float)(this->data[i]); + for (unsigned int i = 0; i < 12; ++i) { + M[i] = static_cast(this->data[i]); + } } /*! @@ -1008,8 +1024,9 @@ void vpHomogeneousMatrix::convert(std::vector &M) void vpHomogeneousMatrix::convert(std::vector &M) { M.resize(12); - for (unsigned int i = 0; i < 12; i++) + for (unsigned int i = 0; i < 12; ++i) { M[i] = this->data[i]; + } } /*! @@ -1073,12 +1090,14 @@ Last column: */ vpColVector vpHomogeneousMatrix::getCol(unsigned int j) const { - if (j >= getCols()) + if (j >= getCols()) { throw(vpException(vpException::dimensionError, "Unable to extract a column vector from the homogeneous matrix")); + } unsigned int nb_rows = getRows(); vpColVector c(nb_rows); - for (unsigned int i = 0; i < nb_rows; i++) + for (unsigned int i = 0; i < nb_rows; ++i) { c[i] = (*this)[i][j]; + } return c; } @@ -1094,14 +1113,15 @@ vpHomogeneousMatrix vpHomogeneousMatrix::compute3d3dTransformation(const std::ve vpColVector p_bar(3, 0.0); vpColVector q_bar(3, 0.0); - for (size_t i = 0; i < p.size(); i++) { - for (unsigned int j = 0; j < 3; j++) { + size_t p_size = p.size(); + for (size_t i = 0; i < p_size; ++i) { + for (unsigned int j = 0; j < 3; ++j) { p_bar[j] += p.at(i).oP[j]; q_bar[j] += q.at(i).oP[j]; } } - for (unsigned int j = 0; j < 3; j++) { + for (unsigned int j = 0; j < 3; ++j) { p_bar[j] /= N; q_bar[j] /= N; } @@ -1109,8 +1129,8 @@ vpHomogeneousMatrix vpHomogeneousMatrix::compute3d3dTransformation(const std::ve vpMatrix pc(static_cast(p.size()), 3); vpMatrix qc(static_cast(q.size()), 3); - for (unsigned int i = 0; i < static_cast(p.size()); i++) { - for (unsigned int j = 0; j < 3; j++) { + for (unsigned int i = 0; i < static_cast(p_size); ++i) { + for (unsigned int j = 0; j < 3; ++j) { pc[i][j] = p.at(i).oP[j] - p_bar[j]; qc[i][j] = q.at(i).oP[j] - q_bar[j]; } @@ -1131,7 +1151,7 @@ vpHomogeneousMatrix vpHomogeneousMatrix::compute3d3dTransformation(const std::ve R = U * Vt; } - const vpColVector t = p_bar - R * q_bar; + const vpColVector t = p_bar - (R * q_bar); return vpHomogeneousMatrix(vpTranslationVector(t), vpRotationMatrix(R)); } @@ -1150,7 +1170,8 @@ vpHomogeneousMatrix vpHomogeneousMatrix::mean(const std::vector> (*this)[i]; } } @@ -426,7 +429,7 @@ int vpPoseVector::print(std::ostream &s, unsigned int length, char const *intro) std::ostringstream ossFixed; std::ios_base::fmtflags original_flags = oss.flags(); - // ossFixed <(maxAfter, totalLength - maxBefore); - if (maxAfter == 1) + if (maxAfter == 1) { maxAfter = 0; + } // the following line is useful for debugging // std::cerr <(maxBefore)); s << values[i].substr(0, p).c_str(); if (maxAfter > 0) { s.setf(std::ios::left, std::ios::adjustfield); if (p != std::string::npos) { - s.width((std::streamsize)maxAfter); + s.width(static_cast(maxAfter)); s << values[i].substr(p, maxAfter).c_str(); } else { assert(maxAfter > 1); - s.width((std::streamsize)maxAfter); + s.width(static_cast(maxAfter)); s << ".0"; } } @@ -497,7 +502,7 @@ int vpPoseVector::print(std::ostream &s, unsigned int length, char const *intro) s.flags(original_flags); // restore s to standard state - return (int)(maxBefore + maxAfter); + return static_cast(maxBefore + maxAfter); } /*! @@ -508,8 +513,10 @@ std::vector vpPoseVector::toStdVector() const { std::vector v(this->size()); - for (unsigned int i = 0; i < this->size(); i++) + unsigned int this_size = this->size(); + for (unsigned int i = 0; i < this_size; ++i) { v[i] = data[i]; + } return v; } @@ -535,7 +542,7 @@ void vpPoseVector::parse_json(const nlohmann::json &j) from_json(j, *asArray); } - if (getCols() != 1 && getRows() != 6) { + if ((getCols() != 1) && (getRows() != 6)) { throw vpException(vpException::badValue, "From JSON, tried to read something that is not a 6D pose vector"); } } diff --git a/modules/core/src/math/transformation/vpQuaternionVector.cpp b/modules/core/src/math/transformation/vpQuaternionVector.cpp index 22eb0687c5..63f3d108ed 100644 --- a/modules/core/src/math/transformation/vpQuaternionVector.cpp +++ b/modules/core/src/math/transformation/vpQuaternionVector.cpp @@ -136,8 +136,9 @@ vpQuaternionVector vpQuaternionVector::buildFrom(const vpColVector &q) throw(vpException(vpException::dimensionError, "Cannot construct a quaternion vector from a %d-dimension col vector", q.size())); } - for (unsigned int i = 0; i < 4; i++) + for (unsigned int i = 0; i < 4; ++i) { data[i] = q[i]; + } return *this; } @@ -151,8 +152,9 @@ vpQuaternionVector vpQuaternionVector::buildFrom(const std::vector &q) throw(vpException(vpException::dimensionError, "Cannot construct a quaternion vector from a %d-dimension std::vector", q.size())); } - for (unsigned int i = 0; i < 4; i++) + for (unsigned int i = 0; i < 4; ++i) { data[i] = q[i]; + } return *this; } @@ -192,10 +194,10 @@ vpQuaternionVector vpQuaternionVector::operator*(double l) const //! Multiply two quaternions. vpQuaternionVector vpQuaternionVector::operator*(const vpQuaternionVector &rq) const { - return vpQuaternionVector(w() * rq.x() + x() * rq.w() + y() * rq.z() - z() * rq.y(), - w() * rq.y() + y() * rq.w() + z() * rq.x() - x() * rq.z(), - w() * rq.z() + z() * rq.w() + x() * rq.y() - y() * rq.x(), - w() * rq.w() - x() * rq.x() - y() * rq.y() - z() * rq.z()); + return vpQuaternionVector(((w() * rq.x()) + (x() * rq.w()) + (y() * rq.z())) - (z() * rq.y()), + ((w() * rq.y()) + (y() * rq.w()) + (z() * rq.x())) - (x() * rq.z()), + ((w() * rq.z()) + (z() * rq.w()) + (x() * rq.y())) - (y() * rq.x()), + ((w() * rq.w()) - (x() * rq.x()) - (y() * rq.y())) - (z() * rq.z())); } //! Division by scalar. Returns a quaternion defined by (x/l,y/l,z/l,w/l). @@ -237,8 +239,9 @@ vpQuaternionVector &vpQuaternionVector::operator=(const vpColVector &q) throw(vpException(vpException::dimensionError, "Cannot set a quaternion vector from a %d-dimension col vector", q.size())); } - for (unsigned int i = 0; i < 4; i++) + for (unsigned int i = 0; i < 4; ++i) { data[i] = q[i]; + } return *this; } @@ -278,7 +281,7 @@ vpQuaternionVector vpQuaternionVector::inverse() const { vpQuaternionVector q_inv; - double mag_square = w() * w() + x() * x() + y() * y() + z() * z(); + double mag_square = (w() * w()) + (x() * x()) + (y() * y()) + (z() * z()); if (!vpMath::nul(mag_square, std::numeric_limits::epsilon())) { q_inv = this->conjugate() / mag_square; } @@ -294,7 +297,7 @@ vpQuaternionVector vpQuaternionVector::inverse() const \return The magnitude or norm of the quaternion. */ -double vpQuaternionVector::magnitude() const { return sqrt(w() * w() + x() * x() + y() * y() + z() * z()); } +double vpQuaternionVector::magnitude() const { return sqrt((w() * w()) + (x() * x()) + (y() * y()) + (z() * z())); } /*! Normalize the quaternion. @@ -317,7 +320,7 @@ void vpQuaternionVector::normalize() */ double vpQuaternionVector::dot(const vpQuaternionVector &q0, const vpQuaternionVector &q1) { - return q0.x() * q1.x() + q0.y() * q1.y() + q0.z() * q1.z() + q0.w() * q1.w(); + return (q0.x() * q1.x()) + (q0.y() * q1.y()) + (q0.z() * q1.z()) + (q0.w() * q1.w()); } //! Returns the x-component of the quaternion. @@ -395,10 +398,10 @@ vpQuaternionVector vpQuaternionVector::lerp(const vpQuaternionVector &q0, const } vpQuaternionVector qLerp; - qLerp.x() = q0.x() - t * (q0.x() - q1.x()); - qLerp.y() = q0.y() - t * (q0.y() - q1.y()); - qLerp.z() = q0.z() - t * (q0.z() - q1.z()); - qLerp.w() = q0.w() - t * (q0.w() - q1.w()); + qLerp.x() = q0.x() - (t * (q0.x() - q1.x())); + qLerp.y() = q0.y() - (t * (q0.y() - q1.y())); + qLerp.z() = q0.z() - (t * (q0.z() - q1.z())); + qLerp.w() = q0.w() - (t * (q0.w() - q1.w())); return qLerp; } @@ -463,12 +466,12 @@ vpQuaternionVector vpQuaternionVector::slerp(const vpQuaternionVector &q0, const double scale0 = 1 - t; double scale1 = t; - if (1 - cosHalfTheta > 0.1) { + if ((1 - cosHalfTheta) > 0.1) { double theta = std::acos(cosHalfTheta); double invSinTheta = 1 / std::sin(theta); scale0 = std::sin((1 - t) * theta) * invSinTheta; - scale1 = std::sin((t * theta)) * invSinTheta; + scale1 = std::sin(t * theta) * invSinTheta; } vpQuaternionVector qSlerp; diff --git a/modules/core/src/math/transformation/vpRotationMatrix.cpp b/modules/core/src/math/transformation/vpRotationMatrix.cpp index 5f13b3ef64..d28681ed5e 100644 --- a/modules/core/src/math/transformation/vpRotationMatrix.cpp +++ b/modules/core/src/math/transformation/vpRotationMatrix.cpp @@ -59,12 +59,14 @@ */ void vpRotationMatrix::eye() { - for (unsigned int i = 0; i < 3; i++) { - for (unsigned int j = 0; j < 3; j++) { - if (i == j) + for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int j = 0; j < 3; ++j) { + if (i == j) { (*this)[i][j] = 1.0; - else + } + else { (*this)[i][j] = 0.0; + } } } } @@ -80,8 +82,8 @@ void vpRotationMatrix::eye() */ vpRotationMatrix &vpRotationMatrix::operator=(const vpRotationMatrix &R) { - 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) { rowPtrs[i][j] = R.rowPtrs[i][j]; } } @@ -160,8 +162,8 @@ vpRotationMatrix &vpRotationMatrix::operator=(const vpMatrix &M) M.getRows(), M.getCols())); } - 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) { (*this)[i][j] = M[i][j]; } } @@ -256,11 +258,12 @@ vpRotationMatrix vpRotationMatrix::operator*(const vpRotationMatrix &R) const { vpRotationMatrix p; - 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) { double s = 0; - for (unsigned int k = 0; k < 3; k++) + for (unsigned int k = 0; k < 3; ++k) { s += rowPtrs[i][k] * R.rowPtrs[k][j]; + } p[i][j] = s; } } @@ -286,17 +289,18 @@ vpRotationMatrix vpRotationMatrix::operator*(const vpRotationMatrix &R) const */ vpMatrix vpRotationMatrix::operator*(const vpMatrix &M) const { - if (M.getRows() != 3 || M.getCols() != 3) { + if ((M.getRows() != 3) || (M.getCols() != 3)) { throw(vpException(vpException::dimensionError, "Cannot set a (3x3) rotation matrix from a (%dx%d) matrix", M.getRows(), M.getCols())); } vpMatrix p(3, 3); - 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) { double s = 0; - for (unsigned int k = 0; k < 3; k++) + for (unsigned int k = 0; k < 3; ++k) { s += (*this)[i][k] * M[k][j]; + } p[i][j] = s; } } @@ -361,9 +365,9 @@ vpColVector vpRotationMatrix::operator*(const vpColVector &v) const } vpColVector v_out(3); - for (unsigned int j = 0; j < colNum; j++) { + for (unsigned int j = 0; j < colNum; ++j) { double vj = v[j]; // optimization em 5/12/2006 - for (unsigned int i = 0; i < rowNum; i++) { + for (unsigned int i = 0; i < rowNum; ++i) { v_out[i] += rowPtrs[i][j] * vj; } } @@ -379,11 +383,12 @@ vpTranslationVector vpRotationMatrix::operator*(const vpTranslationVector &tv) c { vpTranslationVector p; - for (unsigned int j = 0; j < 3; j++) + for (unsigned int j = 0; j < 3; ++j) { p[j] = 0; + } - for (unsigned int j = 0; j < 3; j++) { - for (unsigned int i = 0; i < 3; i++) { + for (unsigned int j = 0; j < 3; ++j) { + for (unsigned int i = 0; i < 3; ++i) { p[i] += rowPtrs[i][j] * tv[j]; } } @@ -399,9 +404,11 @@ vpRotationMatrix vpRotationMatrix::operator*(double x) const { vpRotationMatrix R; - for (unsigned int i = 0; i < rowNum; i++) - for (unsigned int j = 0; j < colNum; j++) + for (unsigned int i = 0; i < rowNum; ++i) { + for (unsigned int j = 0; j < colNum; ++j) { R[i][j] = rowPtrs[i][j] * x; + } + } return R; } @@ -412,9 +419,11 @@ vpRotationMatrix vpRotationMatrix::operator*(double x) const */ vpRotationMatrix &vpRotationMatrix::operator*=(double x) { - for (unsigned int i = 0; i < rowNum; i++) - for (unsigned int j = 0; j < colNum; j++) + for (unsigned int i = 0; i < rowNum; ++i) { + for (unsigned int j = 0; j < colNum; ++j) { rowPtrs[i][j] *= x; + } + } return *this; } @@ -431,14 +440,14 @@ bool vpRotationMatrix::isARotationMatrix(double threshold) const { bool isRotation = true; - if (getCols() != 3 || getRows() != 3) { + if ((getCols() != 3) || (getRows() != 3)) { return false; } - // test R^TR = Id ; + // --comment: test R^TR = Id vpRotationMatrix RtR = (*this).t() * (*this); - 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) { if (i == j) { if (fabs(RtR[i][j] - 1) > threshold) { isRotation = false; @@ -453,14 +462,14 @@ bool vpRotationMatrix::isARotationMatrix(double threshold) const } // test if it is a basis // test || Ci || = 1 - for (unsigned int i = 0; i < 3; i++) { + for (unsigned int i = 0; i < 3; ++i) { if ((sqrt(vpMath::sqr(RtR[0][i]) + vpMath::sqr(RtR[1][i]) + vpMath::sqr(RtR[2][i])) - 1) > threshold) { isRotation = false; } } // test || Ri || = 1 - for (unsigned int i = 0; i < 3; i++) { + for (unsigned int i = 0; i < 3; ++i) { if ((sqrt(vpMath::sqr(RtR[i][0]) + vpMath::sqr(RtR[i][1]) + vpMath::sqr(RtR[i][2])) - 1) > threshold) { isRotation = false; } @@ -585,9 +594,11 @@ vpRotationMatrix vpRotationMatrix::t() const { vpRotationMatrix Rt; - 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) { Rt[j][i] = (*this)[i][j]; + } + } return Rt; } @@ -632,8 +643,9 @@ void vpRotationMatrix::printVector() { vpThetaUVector tu(*this); - for (unsigned int i = 0; i < 3; i++) + for (unsigned int i = 0; i < 3; ++i) { std::cout << tu[i] << " "; + } std::cout << std::endl; } @@ -652,25 +664,27 @@ vpRotationMatrix vpRotationMatrix::buildFrom(const vpThetaUVector &v) double theta, si, co, sinc, mcosc; vpRotationMatrix R; - theta = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]); + theta = sqrt((v[0] * v[0]) + (v[1] * v[1]) + (v[2] * v[2])); si = sin(theta); co = cos(theta); sinc = vpMath::sinc(si, theta); mcosc = vpMath::mcosc(co, theta); - R[0][0] = co + mcosc * v[0] * v[0]; - R[0][1] = -sinc * v[2] + mcosc * v[0] * v[1]; - R[0][2] = sinc * v[1] + mcosc * v[0] * v[2]; - R[1][0] = sinc * v[2] + mcosc * v[1] * v[0]; - R[1][1] = co + mcosc * v[1] * v[1]; - R[1][2] = -sinc * v[0] + mcosc * v[1] * v[2]; - R[2][0] = -sinc * v[1] + mcosc * v[2] * v[0]; - R[2][1] = sinc * v[0] + mcosc * v[2] * v[1]; - R[2][2] = co + mcosc * v[2] * v[2]; - - for (unsigned int i = 0; i < 3; i++) - for (unsigned int j = 0; j < 3; j++) + R[0][0] = co + (mcosc * v[0] * v[0]); + R[0][1] = (-sinc * v[2]) + (mcosc * v[0] * v[1]); + R[0][2] = (sinc * v[1]) + (mcosc * v[0] * v[2]); + R[1][0] = (sinc * v[2]) + (mcosc * v[1] * v[0]); + R[1][1] = co + (mcosc * v[1] * v[1]); + R[1][2] = (-sinc * v[0]) + (mcosc * v[1] * v[2]); + R[2][0] = (-sinc * v[1]) + (mcosc * v[2] * v[0]); + R[2][1] = (sinc * v[0]) + (mcosc * v[2] * v[1]); + R[2][2] = co + (mcosc * v[2] * v[2]); + + for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int j = 0; j < 3; ++j) { (*this)[i][j] = R[i][j]; + } + } return *this; } @@ -680,9 +694,11 @@ vpRotationMatrix vpRotationMatrix::buildFrom(const vpThetaUVector &v) */ vpRotationMatrix vpRotationMatrix::buildFrom(const vpHomogeneousMatrix &M) { - 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) { (*this)[i][j] = M[i][j]; + } + } return *this; } @@ -716,17 +732,17 @@ vpRotationMatrix vpRotationMatrix::buildFrom(const vpRzyzVector &v) s1 = sin(v[1]); s2 = sin(v[2]); - (*this)[0][0] = c0 * c1 * c2 - s0 * s2; - (*this)[0][1] = -c0 * c1 * s2 - s0 * c2; + (*this)[0][0] = (c0 * c1 * c2) - (s0 * s2); + (*this)[0][1] = (-c0 * c1 * s2) - (s0 * c2); (*this)[0][2] = c0 * s1; - (*this)[1][0] = s0 * c1 * c2 + c0 * s2; - (*this)[1][1] = -s0 * c1 * s2 + c0 * c2; + (*this)[1][0] = (s0 * c1 * c2) + (c0 * s2); + (*this)[1][1] = (-s0 * c1 * s2) + (c0 * c2); (*this)[1][2] = s0 * s1; (*this)[2][0] = -s1 * c2; (*this)[2][1] = s1 * s2; (*this)[2][2] = c1; - return (*this); + return *this; } /*! @@ -751,14 +767,14 @@ vpRotationMatrix vpRotationMatrix::buildFrom(const vpRxyzVector &v) (*this)[0][0] = c1 * c2; (*this)[0][1] = -c1 * s2; (*this)[0][2] = s1; - (*this)[1][0] = c0 * s2 + s0 * s1 * c2; - (*this)[1][1] = c0 * c2 - s0 * s1 * s2; + (*this)[1][0] = (c0 * s2) + (s0 * s1 * c2); + (*this)[1][1] = (c0 * c2) - (s0 * s1 * s2); (*this)[1][2] = -s0 * c1; - (*this)[2][0] = -c0 * s1 * c2 + s0 * s2; - (*this)[2][1] = c0 * s1 * s2 + c2 * s0; + (*this)[2][0] = (-c0 * s1 * c2) + (s0 * s2); + (*this)[2][1] = (c0 * s1 * s2) + (c2 * s0); (*this)[2][2] = c0 * c1; - return (*this); + return *this; } /*! @@ -779,18 +795,18 @@ vpRotationMatrix vpRotationMatrix::buildFrom(const vpRzyxVector &v) s2 = sin(v[2]); (*this)[0][0] = c0 * c1; - (*this)[0][1] = c0 * s1 * s2 - s0 * c2; - (*this)[0][2] = c0 * s1 * c2 + s0 * s2; + (*this)[0][1] = (c0 * s1 * s2) - (s0 * c2); + (*this)[0][2] = (c0 * s1 * c2) + (s0 * s2); (*this)[1][0] = s0 * c1; - (*this)[1][1] = s0 * s1 * s2 + c0 * c2; - (*this)[1][2] = s0 * s1 * c2 - c0 * s2; + (*this)[1][1] = (s0 * s1 * s2) + (c0 * c2); + (*this)[1][2] = (s0 * s1 * c2) - (c0 * s2); (*this)[2][0] = -s1; (*this)[2][1] = c1 * s2; (*this)[2][2] = c1 * c2; - return (*this); + return *this; } /*! @@ -813,17 +829,17 @@ vpRotationMatrix vpRotationMatrix::buildFrom(const vpQuaternionVector &q) double b = q.x(); double c = q.y(); double d = q.z(); - (*this)[0][0] = a * a + b * b - c * c - d * d; - (*this)[0][1] = 2 * b * c - 2 * a * d; - (*this)[0][2] = 2 * a * c + 2 * b * d; + (*this)[0][0] = ((a * a) + (b * b) - (c * c)) - (d * d); + (*this)[0][1] = (2 * b * c) - (2 * a * d); + (*this)[0][2] = (2 * a * c) + (2 * b * d); - (*this)[1][0] = 2 * a * d + 2 * b * c; - (*this)[1][1] = a * a - b * b + c * c - d * d; - (*this)[1][2] = 2 * c * d - 2 * a * b; + (*this)[1][0] = (2 * a * d) + (2 * b * c); + (*this)[1][1] = ((a * a) - (b * b) + (c * c)) - (d * d); + (*this)[1][2] = (2 * c * d) - (2 * a * b); - (*this)[2][0] = 2 * b * d - 2 * a * c; - (*this)[2][1] = 2 * a * b + 2 * c * d; - (*this)[2][2] = a * a - b * b - c * c + d * d; + (*this)[2][0] = (2 * b * d) - (2 * a * c); + (*this)[2][1] = (2 * a * b) + (2 * c * d); + (*this)[2][2] = ((a * a) - (b * b) - (c * c)) + (d * d); return *this; } @@ -837,9 +853,11 @@ vpRotationMatrix operator*(const double &x, const vpRotationMatrix &R) unsigned int Rrow = R.getRows(); unsigned int Rcol = R.getCols(); - for (unsigned int i = 0; i < Rrow; i++) - for (unsigned int j = 0; j < Rcol; j++) + for (unsigned int i = 0; i < Rrow; ++i) { + for (unsigned int j = 0; j < Rcol; ++j) { C[i][j] = R[i][j] * x; + } + } return C; } @@ -884,12 +902,14 @@ Last column: */ vpColVector vpRotationMatrix::getCol(unsigned int j) const { - if (j >= getCols()) + if (j >= getCols()) { throw(vpException(vpException::dimensionError, "Unable to extract a column vector from the homogeneous matrix")); + } unsigned int nb_rows = getRows(); vpColVector c(nb_rows); - for (unsigned int i = 0; i < nb_rows; i++) + for (unsigned int i = 0; i < nb_rows; ++i) { c[i] = (*this)[i][j]; + } return c; } @@ -906,7 +926,8 @@ vpRotationMatrix vpRotationMatrix::mean(const std::vector & { vpMatrix meanR(3, 3); vpRotationMatrix R; - for (size_t i = 0; i < vec_M.size(); i++) { + size_t vec_m_size = vec_M.size(); + for (size_t i = 0; i < vec_m_size; ++i) { R = vec_M[i].getRotationMatrix(); meanR += (vpMatrix)R; } @@ -923,7 +944,8 @@ vpRotationMatrix vpRotationMatrix::mean(const std::vector & else { vpMatrix D(3, 3); D = 0.0; - D[0][0] = D[1][1] = 1.0; + D[0][0] = 1.0; + D[1][1] = 1.0; D[2][2] = -1; meanR = U * D * V.t(); } @@ -944,7 +966,8 @@ vpRotationMatrix vpRotationMatrix::mean(const std::vector &vec { vpMatrix meanR(3, 3); vpRotationMatrix R; - for (size_t i = 0; i < vec_R.size(); i++) { + size_t vec_r_size = vec_R.size(); + for (size_t i = 0; i < vec_r_size; ++i) { meanR += (vpMatrix)vec_R[i]; } meanR /= static_cast(vec_R.size()); @@ -960,7 +983,8 @@ vpRotationMatrix vpRotationMatrix::mean(const std::vector &vec else { vpMatrix D(3, 3); D = 0.0; - D[0][0] = D[1][1] = 1.0; + D[0][0] = 1.0; + D[1][1] = 1.0; D[2][2] = -1; meanR = U * D * V.t(); } diff --git a/modules/core/src/math/transformation/vpRotationVector.cpp b/modules/core/src/math/transformation/vpRotationVector.cpp index 4b29b7c144..3c895fccfd 100644 --- a/modules/core/src/math/transformation/vpRotationVector.cpp +++ b/modules/core/src/math/transformation/vpRotationVector.cpp @@ -54,8 +54,9 @@ vpRowVector vpRotationVector::t() const { vpRowVector v(dsize); - for (unsigned int i = 0; i < dsize; i++) + for (unsigned int i = 0; i < dsize; ++i) { v[i] = data[i]; + } return v; } @@ -68,8 +69,10 @@ std::vector vpRotationVector::toStdVector() const { std::vector v(this->size()); - for (unsigned int i = 0; i < this->size(); i++) + unsigned int this_size = this->size(); + for (unsigned int i = 0; i < this_size; ++i) { v[i] = data[i]; + } return v; } @@ -87,8 +90,9 @@ vpColVector vpRotationVector::operator*(double x) const { vpColVector v(dsize); - for (unsigned int i = 0; i < dsize; i++) + for (unsigned int i = 0; i < dsize; ++i) { v[i] = (*this)[i] * x; + } return v; } @@ -179,7 +183,7 @@ double vpRotationVector::sumSquare() const { double sum_square = 0.0; - for (unsigned int i = 0; i < rowNum; i++) { + for (unsigned int i = 0; i < rowNum; ++i) { double x = rowPtrs[i][0]; sum_square += x * x; } diff --git a/modules/core/src/math/transformation/vpRxyzVector.cpp b/modules/core/src/math/transformation/vpRxyzVector.cpp index 704881fde9..a59a2d0db7 100644 --- a/modules/core/src/math/transformation/vpRxyzVector.cpp +++ b/modules/core/src/math/transformation/vpRxyzVector.cpp @@ -90,15 +90,17 @@ vpRxyzVector vpRxyzVector::buildFrom(const vpRotationMatrix &R) double COEF_MIN_ROT = 1e-6; double phi; - if ((fabs(R[1][2]) < COEF_MIN_ROT) && (fabs(R[2][2]) < COEF_MIN_ROT)) + if ((fabs(R[1][2]) < COEF_MIN_ROT) && (fabs(R[2][2]) < COEF_MIN_ROT)) { phi = 0; - else + } + else { phi = atan2(-R[1][2], R[2][2]); + } double si = sin(phi); double co = cos(phi); - double theta = atan2(R[0][2], -si * R[1][2] + co * R[2][2]); - double psi = atan2(co * R[1][0] + si * R[2][0], co * R[1][1] + si * R[2][1]); + double theta = atan2(R[0][2], (-si * R[1][2]) + (co * R[2][2])); + double psi = atan2((co * R[1][0]) + (si * R[2][0]), (co * R[1][1]) + (si * R[2][1])); buildFrom(phi, theta, psi); @@ -142,8 +144,9 @@ vpRxyzVector vpRxyzVector::buildFrom(const vpColVector &rxyz) throw(vpException(vpException::dimensionError, "Cannot construct a R-xyz vector from a %d-dimension col vector", rxyz.size())); } - for (unsigned int i = 0; i < 3; i++) + for (unsigned int i = 0; i < 3; ++i) { data[i] = rxyz[i]; + } return *this; } @@ -157,8 +160,9 @@ vpRxyzVector vpRxyzVector::buildFrom(const std::vector &rxyz) throw(vpException(vpException::dimensionError, "Cannot construct a R-xyz vector from a %d-dimension std::vector", rxyz.size())); } - for (unsigned int i = 0; i < 3; i++) + for (unsigned int i = 0; i < 3; ++i) { data[i] = rxyz[i]; + } return *this; } @@ -184,8 +188,9 @@ vpRxyzVector vpRxyzVector::buildFrom(const std::vector &rxyz) */ vpRxyzVector &vpRxyzVector::operator=(double v) { - for (unsigned int i = 0; i < dsize; i++) + for (unsigned int i = 0; i < dsize; ++i) { data[i] = v; + } return *this; } @@ -218,8 +223,9 @@ vpRxyzVector &vpRxyzVector::operator=(const vpColVector &rxyz) throw(vpException(vpException::dimensionError, "Cannot set a R-xyz vector from a %d-dimension col vector", rxyz.size())); } - for (unsigned int i = 0; i < 3; i++) + for (unsigned int i = 0; i < 3; ++i) { data[i] = rxyz[i]; + } return *this; } diff --git a/modules/core/src/math/transformation/vpRzyxVector.cpp b/modules/core/src/math/transformation/vpRzyxVector.cpp index 3576fd586b..c206c7b8de 100644 --- a/modules/core/src/math/transformation/vpRzyxVector.cpp +++ b/modules/core/src/math/transformation/vpRzyxVector.cpp @@ -106,14 +106,14 @@ vpRzyxVector vpRzyxVector::buildFrom(const vpRotationMatrix &R) double co = cos(phi); double nz = R[2][0]; - double theta = atan2(-nz, co * nx + si * ny); + double theta = atan2(-nz, (co * nx) + (si * ny)); double ax = R[0][2]; double ay = R[1][2]; double ox = R[0][1]; double oy = R[1][1]; - double psi = atan2(si * ax - co * ay, -si * ox + co * oy); + double psi = atan2((si * ax) - (co * ay), (-si * ox) + (co * oy)); buildFrom(phi, theta, psi); @@ -157,8 +157,9 @@ vpRzyxVector vpRzyxVector::buildFrom(const vpColVector &rzyx) throw(vpException(vpException::dimensionError, "Cannot construct a R-zyx vector from a %d-dimension col vector", rzyx.size())); } - for (unsigned int i = 0; i < 3; i++) + for (unsigned int i = 0; i < 3; ++i) { data[i] = rzyx[i]; + } return *this; } @@ -172,8 +173,9 @@ vpRzyxVector vpRzyxVector::buildFrom(const std::vector &rzyx) throw(vpException(vpException::dimensionError, "Cannot construct a R-zyx vector from a %d-dimension std::vector", rzyx.size())); } - for (unsigned int i = 0; i < 3; i++) + for (unsigned int i = 0; i < 3; ++i) { data[i] = rzyx[i]; + } return *this; } @@ -199,8 +201,9 @@ vpRzyxVector vpRzyxVector::buildFrom(const std::vector &rzyx) */ vpRzyxVector &vpRzyxVector::operator=(double v) { - for (unsigned int i = 0; i < dsize; i++) + for (unsigned int i = 0; i < dsize; ++i) { data[i] = v; + } return *this; } @@ -233,8 +236,9 @@ vpRzyxVector &vpRzyxVector::operator=(const vpColVector &rzyx) throw(vpException(vpException::dimensionError, "Cannot set a R-zyx vector from a %d-dimension col vector", rzyx.size())); } - for (unsigned int i = 0; i < 3; i++) + for (unsigned int i = 0; i < 3; ++i) { data[i] = rzyx[i]; + } return *this; } diff --git a/modules/core/src/math/transformation/vpRzyzVector.cpp b/modules/core/src/math/transformation/vpRzyzVector.cpp index f3fe667c06..ee6be2eb13 100644 --- a/modules/core/src/math/transformation/vpRzyzVector.cpp +++ b/modules/core/src/math/transformation/vpRzyzVector.cpp @@ -86,16 +86,18 @@ vpRzyzVector::vpRzyzVector(const std::vector &rzyz) : vpRotationVector(3 vpRzyzVector vpRzyzVector::buildFrom(const vpRotationMatrix &R) { double phi; - if ((fabs(R[1][2]) < 1e-6) && (fabs(R[0][2]) < 1e-6)) + if ((fabs(R[1][2]) < 1e-6) && (fabs(R[0][2]) < 1e-6)) { phi = 0; - else + } + else { phi = atan2(R[1][2], R[0][2]); + } double cphi = cos(phi); double sphi = sin(phi); - double theta = atan2(cphi * R[0][2] + sphi * R[1][2], R[2][2]); + double theta = atan2((cphi * R[0][2]) + (sphi * R[1][2]), R[2][2]); - double psi = atan2(-sphi * R[0][0] + cphi * R[1][0], -sphi * R[0][1] + cphi * R[1][1]); + double psi = atan2((-sphi * R[0][0]) + (cphi * R[1][0]), (-sphi * R[0][1]) + (cphi * R[1][1])); buildFrom(phi, theta, psi); @@ -126,8 +128,9 @@ vpRzyzVector vpRzyzVector::buildFrom(const vpColVector &rzyz) throw(vpException(vpException::dimensionError, "Cannot construct a R-zyz vector from a %d-dimension col vector", rzyz.size())); } - for (unsigned int i = 0; i < 3; i++) + for (unsigned int i = 0; i < 3; ++i) { data[i] = rzyz[i]; + } return *this; } @@ -141,8 +144,9 @@ vpRzyzVector vpRzyzVector::buildFrom(const std::vector &rzyz) throw(vpException(vpException::dimensionError, "Cannot construct a R-zyx vector from a %d-dimension std::vector", rzyz.size())); } - for (unsigned int i = 0; i < 3; i++) + for (unsigned int i = 0; i < 3; ++i) { data[i] = rzyz[i]; + } return *this; } @@ -168,8 +172,9 @@ vpRzyzVector vpRzyzVector::buildFrom(const std::vector &rzyz) */ vpRzyzVector &vpRzyzVector::operator=(double v) { - for (unsigned int i = 0; i < dsize; i++) + for (unsigned int i = 0; i < dsize; ++i) { data[i] = v; + } return *this; } @@ -215,8 +220,9 @@ vpRzyzVector &vpRzyzVector::operator=(const vpColVector &rzyz) throw(vpException(vpException::dimensionError, "Cannot set a R-zyz vector from a %d-dimension col vector", rzyz.size())); } - for (unsigned int i = 0; i < 3; i++) + for (unsigned int i = 0; i < 3; ++i) { data[i] = rzyz[i]; + } return *this; } diff --git a/modules/core/src/math/transformation/vpThetaUVector.cpp b/modules/core/src/math/transformation/vpThetaUVector.cpp index 3f4ae7355a..7fb9e47ce1 100644 --- a/modules/core/src/math/transformation/vpThetaUVector.cpp +++ b/modules/core/src/math/transformation/vpThetaUVector.cpp @@ -121,8 +121,9 @@ vpThetaUVector vpThetaUVector::buildFrom(const vpHomogeneousMatrix &M) */ vpThetaUVector vpThetaUVector::buildFrom(const vpPoseVector &p) { - for (unsigned int i = 0; i < 3; i++) + for (unsigned int i = 0; i < 3; ++i) { data[i] = p[i + 3]; + } return *this; } @@ -134,10 +135,10 @@ vpThetaUVector vpThetaUVector::buildFrom(const vpRotationMatrix &R) { double s, c, theta; - s = (R[1][0] - R[0][1]) * (R[1][0] - R[0][1]) + (R[2][0] - R[0][2]) * (R[2][0] - R[0][2]) + - (R[2][1] - R[1][2]) * (R[2][1] - R[1][2]); + s = ((R[1][0] - R[0][1]) * (R[1][0] - R[0][1])) + ((R[2][0] - R[0][2]) * (R[2][0] - R[0][2])) + + ((R[2][1] - R[1][2]) * (R[2][1] - R[1][2])); s = sqrt(s) / 2.0; - c = (R[0][0] + R[1][1] + R[2][2] - 1.0) / 2.0; + c = ((R[0][0] + R[1][1] + R[2][2]) - 1.0) / 2.0; theta = atan2(s, c); /* theta in [0, PI] since s > 0 */ // General case when theta != pi. If theta=pi, c=-1 @@ -152,40 +153,52 @@ vpThetaUVector vpThetaUVector::buildFrom(const vpRotationMatrix &R) else /* theta near PI */ { double x = 0; - if ((R[0][0] - c) > std::numeric_limits::epsilon()) + if ((R[0][0] - c) > std::numeric_limits::epsilon()) { x = sqrt((R[0][0] - c) / (1 - c)); + } double y = 0; - if ((R[1][1] - c) > std::numeric_limits::epsilon()) + if ((R[1][1] - c) > std::numeric_limits::epsilon()) { y = sqrt((R[1][1] - c) / (1 - c)); + } double z = 0; - if ((R[2][2] - c) > std::numeric_limits::epsilon()) + if ((R[2][2] - c) > std::numeric_limits::epsilon()) { z = sqrt((R[2][2] - c) / (1 - c)); + } - if (x > y && x > z) { - if ((R[2][1] - R[1][2]) < 0) + if ((x > y) && (x > z)) { + if ((R[2][1] - R[1][2]) < 0) { x = -x; - if (vpMath::sign(x) * vpMath::sign(y) != vpMath::sign(R[0][1] + R[1][0])) + } + if ((vpMath::sign(x) * vpMath::sign(y)) != (vpMath::sign(R[0][1] + R[1][0]))) { y = -y; - if (vpMath::sign(x) * vpMath::sign(z) != vpMath::sign(R[0][2] + R[2][0])) + } + if ((vpMath::sign(x) * vpMath::sign(z)) != (vpMath::sign(R[0][2] + R[2][0]))) { z = -z; + } } else if (y > z) { - if ((R[0][2] - R[2][0]) < 0) + if ((R[0][2] - R[2][0]) < 0) { y = -y; - if (vpMath::sign(y) * vpMath::sign(x) != vpMath::sign(R[1][0] + R[0][1])) + } + if ((vpMath::sign(y) * vpMath::sign(x)) != (vpMath::sign(R[1][0] + R[0][1]))) { x = -x; - if (vpMath::sign(y) * vpMath::sign(z) != vpMath::sign(R[1][2] + R[2][1])) + } + if ((vpMath::sign(y) * vpMath::sign(z)) != (vpMath::sign(R[1][2] + R[2][1]))) { z = -z; + } } else { - if ((R[1][0] - R[0][1]) < 0) + if ((R[1][0] - R[0][1]) < 0) { z = -z; - if (vpMath::sign(z) * vpMath::sign(x) != vpMath::sign(R[2][0] + R[0][2])) + } + if ((vpMath::sign(z) * vpMath::sign(x)) != (vpMath::sign(R[2][0] + R[0][2]))) { x = -x; - if (vpMath::sign(z) * vpMath::sign(y) != vpMath::sign(R[2][1] + R[1][2])) + } + if ((vpMath::sign(z) * vpMath::sign(y)) != (vpMath::sign(R[2][1] + R[1][2]))) { y = -y; + } } data[0] = theta * x; data[1] = theta * y; @@ -245,8 +258,9 @@ vpThetaUVector vpThetaUVector::buildFrom(const std::vector &tu) throw(vpException(vpException::dimensionError, "Cannot construct a theta-u vector from a %d-dimension std::vector", tu.size())); } - for (unsigned int i = 0; i < 3; i++) + for (unsigned int i = 0; i < 3; ++i) { data[i] = tu[i]; + } return *this; } @@ -260,8 +274,9 @@ vpThetaUVector vpThetaUVector::buildFrom(const vpColVector &tu) throw(vpException(vpException::dimensionError, "Cannot construct a theta-u vector from a %d-dimension std::vector", tu.size())); } - for (unsigned int i = 0; i < 3; i++) + for (unsigned int i = 0; i < 3; ++i) { data[i] = tu[i]; + } return *this; } @@ -288,8 +303,9 @@ vpThetaUVector vpThetaUVector::buildFrom(const vpColVector &tu) */ vpThetaUVector &vpThetaUVector::operator=(double v) { - for (unsigned int i = 0; i < dsize; i++) + for (unsigned int i = 0; i < dsize; ++i) { data[i] = v; + } return *this; } @@ -322,8 +338,11 @@ vpThetaUVector &vpThetaUVector::operator=(const vpColVector &tu) throw(vpException(vpException::dimensionError, "Cannot set a theta-u vector from a %d-dimension col vector", tu.size())); } - for (unsigned int i = 0; i < size(); i++) + + unsigned int l_size = size(); + for (unsigned int i = 0; i < l_size; ++i) { data[i] = tu[i]; + } return *this; } @@ -361,13 +380,14 @@ void vpThetaUVector::extract(double &theta, vpColVector &u) const u.resize(3); theta = getTheta(); - // if (theta == 0) { + // --comment: if (theta == 0) if (std::fabs(theta) <= std::numeric_limits::epsilon()) { u = 0; return; } - for (unsigned int i = 0; i < 3; i++) + for (unsigned int i = 0; i < 3; ++i) { u[i] = data[i] / theta; + } } /*! @@ -392,7 +412,7 @@ void vpThetaUVector::extract(double &theta, vpColVector &u) const \sa getTheta(), extract() */ -double vpThetaUVector::getTheta() const { return sqrt(data[0] * data[0] + data[1] * data[1] + data[2] * data[2]); } +double vpThetaUVector::getTheta() const { return sqrt((data[0] * data[0]) + (data[1] * data[1]) + (data[2] * data[2])); } /*! @@ -422,13 +442,14 @@ vpColVector vpThetaUVector::getU() const vpColVector u(3); double theta = getTheta(); - // if (theta == 0) { + // --comment: if (theta == 0) if (std::fabs(theta) <= std::numeric_limits::epsilon()) { u = 0; return u; } - for (unsigned int i = 0; i < 3; i++) + for (unsigned int i = 0; i < 3; ++i) { u[i] = data[i] / theta; + } return u; } @@ -455,10 +476,10 @@ vpThetaUVector vpThetaUVector::operator*(const vpThetaUVector &tu_b) const vpColVector a_hat_sin_2 = a_hat * std::sin(a_2); vpColVector b_hat_sin_2 = b_hat * std::sin(b_2); - double c = 2 * std::acos(std::cos(a_2) * std::cos(b_2) - vpColVector::dotProd(a_hat_sin_2, b_hat_sin_2)); - vpColVector d = std::sin(a_2) * std::cos(b_2) * a_hat + std::cos(a_2) * std::sin(b_2) * b_hat + - std::sin(a_2) * std::sin(b_2) * vpColVector::crossProd(a_hat, b_hat); - d = c * d / std::sin(c / 2); + double c = 2 * std::acos((std::cos(a_2) * std::cos(b_2)) - (vpColVector::dotProd(a_hat_sin_2, b_hat_sin_2))); + vpColVector d = (std::sin(a_2) * std::cos(b_2) * a_hat + std::cos(a_2) * std::sin(b_2) * b_hat) + + ((std::sin(a_2) * std::sin(b_2) * vpColVector::crossProd(a_hat, b_hat))); + d = (c * d) / std::sin(c / 2); return vpThetaUVector(d); } diff --git a/modules/core/src/math/transformation/vpTranslationVector.cpp b/modules/core/src/math/transformation/vpTranslationVector.cpp index 74a22cfa14..80cf5298c6 100644 --- a/modules/core/src/math/transformation/vpTranslationVector.cpp +++ b/modules/core/src/math/transformation/vpTranslationVector.cpp @@ -223,8 +223,9 @@ vpTranslationVector vpTranslationVector::operator+(const vpTranslationVector &tv { vpTranslationVector s; - for (unsigned int i = 0; i < 3; i++) + for (unsigned int i = 0; i < 3; ++i) { s[i] = (*this)[i] + tv[i]; + } return s; } @@ -257,8 +258,9 @@ vpTranslationVector vpTranslationVector::operator+(const vpColVector &v) const } vpTranslationVector s; - for (unsigned int i = 0; i < 3; i++) + for (unsigned int i = 0; i < 3; ++i) { s[i] = (*this)[i] + v[i]; + } return s; } @@ -285,8 +287,9 @@ vpTranslationVector vpTranslationVector::operator-(const vpTranslationVector &tv { vpTranslationVector sub; - for (unsigned int i = 0; i < 3; i++) + for (unsigned int i = 0; i < 3; ++i) { sub[i] = (*this)[i] - tv[i]; + } return sub; } @@ -308,7 +311,7 @@ vpTranslationVector vpTranslationVector::operator-(const vpTranslationVector &tv vpTranslationVector vpTranslationVector::operator-() const // negate { vpTranslationVector tv; - for (unsigned int i = 0; i < dsize; i++) { + for (unsigned int i = 0; i < dsize; ++i) { *(tv.data + i) = -*(data + i); } @@ -334,7 +337,7 @@ vpTranslationVector vpTranslationVector::operator-() const // negate vpTranslationVector vpTranslationVector::operator*(double x) const { vpTranslationVector tv; - for (unsigned int i = 0; i < dsize; i++) { + for (unsigned int i = 0; i < dsize; ++i) { *(tv.data + i) = (*(data + i)) * x; } @@ -353,8 +356,9 @@ vpTranslationVector vpTranslationVector::operator*(double x) const vpMatrix vpTranslationVector::operator*(const vpRowVector &v) const { vpMatrix M(rowNum, v.getCols()); - for (unsigned int i = 0; i < rowNum; i++) { - for (unsigned int j = 0; j < v.getCols(); j++) { + unsigned int v_col = v.getCols(); + for (unsigned int i = 0; i < rowNum; ++i) { + for (unsigned int j = 0; j < v_col; ++j) { M[i][j] = (*this)[i] * v[j]; } } @@ -371,9 +375,10 @@ vpMatrix vpTranslationVector::operator*(const vpRowVector &v) const */ vpTranslationVector &vpTranslationVector::operator*=(double x) { - for (unsigned int i = 0; i < rowNum; i++) + for (unsigned int i = 0; i < rowNum; ++i) { (*this)[i] *= x; - return (*this); + } + return *this; } /*! Operator that allows to divide each element of a translation vector by a @@ -385,9 +390,10 @@ vpTranslationVector &vpTranslationVector::operator*=(double x) */ vpTranslationVector &vpTranslationVector::operator/=(double x) { - for (unsigned int i = 0; i < rowNum; i++) + for (unsigned int i = 0; i < rowNum; ++i) { (*this)[i] /= x; - return (*this); + } + return *this; } /*! @@ -409,7 +415,7 @@ vpTranslationVector &vpTranslationVector::operator/=(double x) vpTranslationVector vpTranslationVector::operator/(double x) const { vpTranslationVector tv; - for (unsigned int i = 0; i < dsize; i++) { + for (unsigned int i = 0; i < dsize; ++i) { *(tv.data + i) = (*(data + i)) / x; } @@ -499,8 +505,9 @@ vpTranslationVector &vpTranslationVector::operator=(double x) { double *d = data; - for (int i = 0; i < 3; i++) + for (int i = 0; i < 3; ++i) { *(d++) = x; + } return *this; } @@ -746,7 +753,7 @@ double vpTranslationVector::sumSquare() const { double sum_square = 0.0; - for (unsigned int i = 0; i < rowNum; i++) { + for (unsigned int i = 0; i < rowNum; ++i) { double x = rowPtrs[i][0]; sum_square += x * x; } @@ -765,7 +772,8 @@ double vpTranslationVector::sumSquare() const vpTranslationVector vpTranslationVector::mean(const std::vector &vec_M) { vpColVector meanT(3); - for (size_t i = 0; i < vec_M.size(); i++) { + size_t vec_m_size = vec_M.size(); + for (size_t i = 0; i < vec_m_size; ++i) { meanT += (vpColVector)vec_M[i].getTranslationVector(); } meanT /= static_cast(vec_M.size()); @@ -785,7 +793,8 @@ vpTranslationVector vpTranslationVector::mean(const std::vector &vec_t) { vpColVector meanT(3); - for (size_t i = 0; i < vec_t.size(); i++) { + size_t l_vec_t_size = vec_t.size(); + for (size_t i = 0; i < l_vec_t_size; ++i) { meanT += (vpColVector)vec_t[i]; } meanT /= static_cast(vec_t.size()); diff --git a/modules/core/src/math/transformation/vpVelocityTwistMatrix.cpp b/modules/core/src/math/transformation/vpVelocityTwistMatrix.cpp index fddb007238..c2ba8f4a66 100644 --- a/modules/core/src/math/transformation/vpVelocityTwistMatrix.cpp +++ b/modules/core/src/math/transformation/vpVelocityTwistMatrix.cpp @@ -54,8 +54,8 @@ */ vpVelocityTwistMatrix &vpVelocityTwistMatrix::operator=(const vpVelocityTwistMatrix &V) { - for (int i = 0; i < 6; i++) { - for (int j = 0; j < 6; j++) { + for (int i = 0; i < 6; ++i) { + for (int j = 0; j < 6; ++j) { rowPtrs[i][j] = V.rowPtrs[i][j]; } } @@ -68,12 +68,16 @@ vpVelocityTwistMatrix &vpVelocityTwistMatrix::operator=(const vpVelocityTwistMat */ void vpVelocityTwistMatrix::eye() { - for (unsigned int i = 0; i < 6; i++) - for (unsigned int j = 0; j < 6; j++) - if (i == j) + for (unsigned int i = 0; i < 6; ++i) { + for (unsigned int j = 0; j < 6; ++j) { + if (i == j) { (*this)[i][j] = 1.0; - else + } + else { (*this)[i][j] = 0.0; + } + } + } } /*! @@ -109,10 +113,12 @@ vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpVelocityTwistMatrix &V) : v */ vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpHomogeneousMatrix &M, bool full) : vpArray2D(6, 6) { - if (full) + if (full) { buildFrom(M); - else + } + else { buildFrom(M.getRotationMatrix()); + } } /*! @@ -217,11 +223,12 @@ vpVelocityTwistMatrix vpVelocityTwistMatrix::operator*(const vpVelocityTwistMatr { vpVelocityTwistMatrix p; - for (unsigned int i = 0; i < 6; i++) { - for (unsigned int j = 0; j < 6; j++) { + for (unsigned int i = 0; i < 6; ++i) { + for (unsigned int j = 0; j < 6; ++j) { double s = 0; - for (int k = 0; k < 6; k++) + for (int k = 0; k < 6; ++k) { s += rowPtrs[i][k] * V.rowPtrs[k][j]; + } p[i][j] = s; } } @@ -275,11 +282,13 @@ vpMatrix vpVelocityTwistMatrix::operator*(const vpMatrix &M) const } vpMatrix p(6, M.getCols()); - for (unsigned int i = 0; i < 6; i++) { - for (unsigned int j = 0; j < M.getCols(); j++) { + unsigned int m_col = M.getCols(); + for (unsigned int i = 0; i < 6; ++i) { + for (unsigned int j = 0; j < m_col; ++j) { double s = 0; - for (unsigned int k = 0; k < 6; k++) + for (unsigned int k = 0; k < 6; ++k) { s += rowPtrs[i][k] * M[k][j]; + } p[i][j] = s; } } @@ -310,11 +319,9 @@ vpColVector vpVelocityTwistMatrix::operator*(const vpColVector &v) const c = 0.0; - for (unsigned int i = 0; i < 6; i++) { - for (unsigned int j = 0; j < 6; j++) { - { - c[i] += rowPtrs[i][j] * v[j]; - } + for (unsigned int i = 0; i < 6; ++i) { + for (unsigned int j = 0; j < 6; ++j) { + c[i] += rowPtrs[i][j] * v[j]; } } @@ -334,14 +341,14 @@ vpColVector vpVelocityTwistMatrix::operator*(const vpColVector &v) const */ vpVelocityTwistMatrix vpVelocityTwistMatrix::buildFrom(const vpRotationMatrix &R) { - 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) { (*this)[i][j] = R[i][j]; (*this)[i + 3][j + 3] = R[i][j]; (*this)[i][j + 3] = 0; } } - return (*this); + return *this; } /*! @@ -362,15 +369,15 @@ vpVelocityTwistMatrix vpVelocityTwistMatrix::buildFrom(const vpTranslationVector { vpMatrix skewaR = t.skew(t) * R; - 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) { (*this)[i][j] = R[i][j]; (*this)[i + 3][j + 3] = R[i][j]; (*this)[i][j + 3] = skewaR[i][j]; } } - return (*this); + return *this; } /*! @@ -391,7 +398,7 @@ vpVelocityTwistMatrix vpVelocityTwistMatrix::buildFrom(const vpTranslationVector vpVelocityTwistMatrix vpVelocityTwistMatrix::buildFrom(const vpTranslationVector &t, const vpThetaUVector &thetau) { buildFrom(t, vpRotationMatrix(thetau)); - return (*this); + return *this; } /*! @@ -409,7 +416,7 @@ vpVelocityTwistMatrix vpVelocityTwistMatrix::buildFrom(const vpTranslationVector vpVelocityTwistMatrix vpVelocityTwistMatrix::buildFrom(const vpThetaUVector &thetau) { buildFrom(vpRotationMatrix(thetau)); - return (*this); + return *this; } /*! @@ -432,12 +439,14 @@ vpVelocityTwistMatrix vpVelocityTwistMatrix::buildFrom(const vpThetaUVector &the */ vpVelocityTwistMatrix vpVelocityTwistMatrix::buildFrom(const vpHomogeneousMatrix &M, bool full) { - if (full) + if (full) { buildFrom(M.getTranslationVector(), M.getRotationMatrix()); - else + } + else { buildFrom(M.getRotationMatrix()); + } - return (*this); + return *this; } //! Invert the velocity twist matrix. @@ -462,9 +471,11 @@ void vpVelocityTwistMatrix::inverse(vpVelocityTwistMatrix &V) const { V = invers //! Extract the rotation matrix from the velocity twist matrix. void vpVelocityTwistMatrix::extract(vpRotationMatrix &R) const { - 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) { R[i][j] = (*this)[i][j]; + } + } } //! Extract the translation vector from the velocity twist matrix. @@ -473,9 +484,11 @@ void vpVelocityTwistMatrix::extract(vpTranslationVector &tv) const vpRotationMatrix R; extract(R); vpMatrix skTR(3, 3); - 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) { skTR[i][j] = (*this)[i][j + 3]; + } + } vpMatrix skT = skTR * R.t(); tv[0] = skT[2][1]; @@ -514,7 +527,7 @@ int vpVelocityTwistMatrix::print(std::ostream &s, unsigned int length, char cons std::ostringstream ossFixed; std::ios_base::fmtflags original_flags = oss.flags(); - // ossFixed <(maxAfter, totalLength - maxBefore); - if (maxAfter == 1) + if (maxAfter == 1) { maxAfter = 0; + } // the following line is useful for debugging // std::cerr <(maxBefore)); + s << values[(i * n) + j].substr(0, p).c_str(); if (maxAfter > 0) { s.setf(std::ios::left, std::ios::adjustfield); if (p != std::string::npos) { - s.width((std::streamsize)maxAfter); - s << values[i * n + j].substr(p, maxAfter).c_str(); + s.width(static_cast(maxAfter)); + s << values[(i * n) + j].substr(p, maxAfter).c_str(); } else { assert(maxAfter > 1); - s.width((std::streamsize)maxAfter); + s.width(static_cast(maxAfter)); s << ".0"; } } @@ -588,7 +603,7 @@ int vpVelocityTwistMatrix::print(std::ostream &s, unsigned int length, char cons s.flags(original_flags); // restore s to standard state - return (int)(maxBefore + maxAfter); + return static_cast(maxBefore + maxAfter); } #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) diff --git a/modules/core/src/math/transformation/vpXmlParserHomogeneousMatrix.cpp b/modules/core/src/math/transformation/vpXmlParserHomogeneousMatrix.cpp index 10a23d4fb7..9d193fdd04 100644 --- a/modules/core/src/math/transformation/vpXmlParserHomogeneousMatrix.cpp +++ b/modules/core/src/math/transformation/vpXmlParserHomogeneousMatrix.cpp @@ -132,11 +132,13 @@ class vpXmlParserHomogeneousMatrix::Impl } if (prop == CODE_XML_M) { - if (SEQUENCE_OK == read_matrix(node, name)) + if (SEQUENCE_OK == read_matrix(node, name)) { nbM++; + } } - else + else { back = SEQUENCE_ERROR; + } } if (nbM == 0) { @@ -213,8 +215,8 @@ class vpXmlParserHomogeneousMatrix::Impl } else { this->m_M = M_tmp; - // std::cout << "Convert in Homogeneous Matrix:"<< std::endl; - // std::cout << this-> M << std::endl; + // --comment: std::cout << "Convert in Homogeneous Matrix:"<< std::endl; + // --comment: std::cout << this-> M << std::endl; this->m_name = M_name_tmp; } return back; @@ -301,13 +303,13 @@ class vpXmlParserHomogeneousMatrix::Impl // Create the Homogeneous matrix M.buildFrom(tx_, ty_, tz_, tux_, tuy_, tuz_); - // std::cout << "Read values from file:" << std::endl; - // std::cout << "tx:" << tx_<< std::endl; - // std::cout << "ty:" << ty_<< std::endl; - // std::cout << "tz:" << tz_<< std::endl; - // std::cout << "tux:" << tux_<< std::endl; - // std::cout << "tuy:" << tuy_<< std::endl; - // std::cout << "tuz:" << tuz_<< std::endl; + // --comment: std::cout << "Read values from file:" << std::endl; + // --comment: std::cout << "tx:" << tx_<< std::endl; + // --comment: std::cout << "ty:" << ty_<< std::endl; + // --comment: std::cout << "tz:" << tz_<< std::endl; + // --comment: std::cout << "tux:" << tux_<< std::endl; + // --comment: std::cout << "tuy:" << tuy_<< std::endl; + // --comment: std::cout << "tuz:" << tuz_<< std::endl; return back; } @@ -375,8 +377,9 @@ class vpXmlParserHomogeneousMatrix::Impl prop = CODE_XML_OTHER; } if (prop == CODE_XML_M) { - if (SEQUENCE_OK == read_matrix(node, name)) + if (SEQUENCE_OK == read_matrix(node, name)) { nbM++; + } } } diff --git a/modules/core/src/tools/file/basisu_miniz.h b/modules/core/src/tools/file/basisu_miniz.h index 0b3cc551a2..187b8c9bfd 100644 --- a/modules/core/src/tools/file/basisu_miniz.h +++ b/modules/core/src/tools/file/basisu_miniz.h @@ -1,8 +1,8 @@ /* miniz.c v1.15 - deflate/inflate, zlib-subset, ZIP reading/writing/appending, PNG writing Implements RFC 1950: http://www.ietf.org/rfc/rfc1950.txt and RFC 1951: http://www.ietf.org/rfc/rfc1951.txt - - Forked from the public domain/unlicense version at: https://code.google.com/archive/p/miniz/ - + + Forked from the public domain/unlicense version at: https://code.google.com/archive/p/miniz/ + Copyright (C) 2019-2021 Binomial LLC. All Rights Reserved. Licensed under the Apache License, Version 2.0 (the "License"); @@ -54,11 +54,11 @@ #if defined(__TINYC__) && (defined(__linux) || defined(__linux__)) // TODO: Work around "error: include file 'sys\utime.h' when compiling with tcc on Linux - #define MINIZ_NO_TIME +#define MINIZ_NO_TIME #endif #if !defined(MINIZ_NO_TIME) && !defined(MINIZ_NO_ARCHIVE_APIS) - #include +#include #endif #if defined(_M_IX86) || defined(_M_X64) || defined(__i386__) || defined(__i386) || defined(__i486__) || defined(__i486) || defined(i386) || defined(__ia64__) || defined(__x86_64__) @@ -89,7 +89,8 @@ #define MINIZ_HAS_64BIT_REGISTERS 1 #endif -namespace buminiz { +namespace buminiz +{ // ------------------- zlib-style API Definitions. @@ -259,76 +260,76 @@ const char *mz_error(int err); // Redefine zlib-compatible names to miniz equivalents, so miniz.c can be used as a drop-in replacement for the subset of zlib that miniz.c supports. // Define MINIZ_NO_ZLIB_COMPATIBLE_NAMES to disable zlib-compatibility if you use zlib in the same project. #ifndef MINIZ_NO_ZLIB_COMPATIBLE_NAMES - typedef unsigned char Byte; - typedef unsigned int uInt; - typedef mz_ulong uLong; - typedef Byte Bytef; - typedef uInt uIntf; - typedef char charf; - typedef int intf; - typedef void *voidpf; - typedef uLong uLongf; - typedef void *voidp; - typedef void *const voidpc; - #define Z_NULL 0 - #define Z_NO_FLUSH MZ_NO_FLUSH - #define Z_PARTIAL_FLUSH MZ_PARTIAL_FLUSH - #define Z_SYNC_FLUSH MZ_SYNC_FLUSH - #define Z_FULL_FLUSH MZ_FULL_FLUSH - #define Z_FINISH MZ_FINISH - #define Z_BLOCK MZ_BLOCK - #define Z_OK MZ_OK - #define Z_STREAM_END MZ_STREAM_END - #define Z_NEED_DICT MZ_NEED_DICT - #define Z_ERRNO MZ_ERRNO - #define Z_STREAM_ERROR MZ_STREAM_ERROR - #define Z_DATA_ERROR MZ_DATA_ERROR - #define Z_MEM_ERROR MZ_MEM_ERROR - #define Z_BUF_ERROR MZ_BUF_ERROR - #define Z_VERSION_ERROR MZ_VERSION_ERROR - #define Z_PARAM_ERROR MZ_PARAM_ERROR - #define Z_NO_COMPRESSION MZ_NO_COMPRESSION - #define Z_BEST_SPEED MZ_BEST_SPEED - #define Z_BEST_COMPRESSION MZ_BEST_COMPRESSION - #define Z_DEFAULT_COMPRESSION MZ_DEFAULT_COMPRESSION - #define Z_DEFAULT_STRATEGY MZ_DEFAULT_STRATEGY - #define Z_FILTERED MZ_FILTERED - #define Z_HUFFMAN_ONLY MZ_HUFFMAN_ONLY - #define Z_RLE MZ_RLE - #define Z_FIXED MZ_FIXED - #define Z_DEFLATED MZ_DEFLATED - #define Z_DEFAULT_WINDOW_BITS MZ_DEFAULT_WINDOW_BITS - #define alloc_func mz_alloc_func - #define free_func mz_free_func - #define internal_state mz_internal_state - #define z_stream mz_stream - #define deflateInit mz_deflateInit - #define deflateInit2 mz_deflateInit2 - #define deflateReset mz_deflateReset - #define deflate mz_deflate - #define deflateEnd mz_deflateEnd - #define deflateBound mz_deflateBound - #define compress mz_compress - #define compress2 mz_compress2 - #define compressBound mz_compressBound - #define inflateInit mz_inflateInit - #define inflateInit2 mz_inflateInit2 - #define inflate mz_inflate - #define inflateEnd mz_inflateEnd - #define uncompress mz_uncompress - #define crc32 mz_crc32 - #define adler32 mz_adler32 - #define MAX_WBITS 15 - #define MAX_MEM_LEVEL 9 - #define zError mz_error - #define ZLIB_VERSION MZ_VERSION - #define ZLIB_VERNUM MZ_VERNUM - #define ZLIB_VER_MAJOR MZ_VER_MAJOR - #define ZLIB_VER_MINOR MZ_VER_MINOR - #define ZLIB_VER_REVISION MZ_VER_REVISION - #define ZLIB_VER_SUBREVISION MZ_VER_SUBREVISION - #define zlibVersion mz_version - #define zlib_version mz_version() +typedef unsigned char Byte; +typedef unsigned int uInt; +typedef mz_ulong uLong; +typedef Byte Bytef; +typedef uInt uIntf; +typedef char charf; +typedef int intf; +typedef void *voidpf; +typedef uLong uLongf; +typedef void *voidp; +typedef void *const voidpc; +#define Z_NULL 0 +#define Z_NO_FLUSH MZ_NO_FLUSH +#define Z_PARTIAL_FLUSH MZ_PARTIAL_FLUSH +#define Z_SYNC_FLUSH MZ_SYNC_FLUSH +#define Z_FULL_FLUSH MZ_FULL_FLUSH +#define Z_FINISH MZ_FINISH +#define Z_BLOCK MZ_BLOCK +#define Z_OK MZ_OK +#define Z_STREAM_END MZ_STREAM_END +#define Z_NEED_DICT MZ_NEED_DICT +#define Z_ERRNO MZ_ERRNO +#define Z_STREAM_ERROR MZ_STREAM_ERROR +#define Z_DATA_ERROR MZ_DATA_ERROR +#define Z_MEM_ERROR MZ_MEM_ERROR +#define Z_BUF_ERROR MZ_BUF_ERROR +#define Z_VERSION_ERROR MZ_VERSION_ERROR +#define Z_PARAM_ERROR MZ_PARAM_ERROR +#define Z_NO_COMPRESSION MZ_NO_COMPRESSION +#define Z_BEST_SPEED MZ_BEST_SPEED +#define Z_BEST_COMPRESSION MZ_BEST_COMPRESSION +#define Z_DEFAULT_COMPRESSION MZ_DEFAULT_COMPRESSION +#define Z_DEFAULT_STRATEGY MZ_DEFAULT_STRATEGY +#define Z_FILTERED MZ_FILTERED +#define Z_HUFFMAN_ONLY MZ_HUFFMAN_ONLY +#define Z_RLE MZ_RLE +#define Z_FIXED MZ_FIXED +#define Z_DEFLATED MZ_DEFLATED +#define Z_DEFAULT_WINDOW_BITS MZ_DEFAULT_WINDOW_BITS +#define alloc_func mz_alloc_func +#define free_func mz_free_func +#define internal_state mz_internal_state +#define z_stream mz_stream +#define deflateInit mz_deflateInit +#define deflateInit2 mz_deflateInit2 +#define deflateReset mz_deflateReset +#define deflate mz_deflate +#define deflateEnd mz_deflateEnd +#define deflateBound mz_deflateBound +#define compress mz_compress +#define compress2 mz_compress2 +#define compressBound mz_compressBound +#define inflateInit mz_inflateInit +#define inflateInit2 mz_inflateInit2 +#define inflate mz_inflate +#define inflateEnd mz_inflateEnd +#define uncompress mz_uncompress +#define crc32 mz_crc32 +#define adler32 mz_adler32 +#define MAX_WBITS 15 +#define MAX_MEM_LEVEL 9 +#define zError mz_error +#define ZLIB_VERSION MZ_VERSION +#define ZLIB_VERNUM MZ_VERNUM +#define ZLIB_VER_MAJOR MZ_VER_MAJOR +#define ZLIB_VER_MINOR MZ_VER_MINOR +#define ZLIB_VER_REVISION MZ_VER_REVISION +#define ZLIB_VER_SUBREVISION MZ_VER_SUBREVISION +#define zlibVersion mz_version +#define zlib_version mz_version() #endif // #ifndef MINIZ_NO_ZLIB_COMPATIBLE_NAMES #endif // MINIZ_NO_ZLIB_APIS @@ -349,9 +350,9 @@ typedef int mz_bool; // An attempt to work around MSVC's spammy "warning C4127: conditional expression is constant" message. #ifdef _MSC_VER - #define MZ_MACRO_END while (0, 0) +#define MZ_MACRO_END while (0, 0) #else - #define MZ_MACRO_END while (0) +#define MZ_MACRO_END while (0) #endif // ------------------- Low-level Decompression API Definitions @@ -386,7 +387,7 @@ size_t tinfl_decompress_mem_to_mem(void *pOut_buf, size_t out_buf_len, const voi // tinfl_decompress_mem_to_callback() decompresses a block in memory to an internal 32KB buffer, and a user provided callback function will be called to flush the buffer. // Returns 1 on success or 0 on failure. -typedef int (*tinfl_put_buf_func_ptr)(const void* pBuf, int len, void *pUser); +typedef int (*tinfl_put_buf_func_ptr)(const void *pBuf, int len, void *pUser); int tinfl_decompress_mem_to_callback(const void *pIn_buf, size_t *pIn_buf_size, tinfl_put_buf_func_ptr pPut_buf_func, void *pPut_buf_user, int flags); struct tinfl_decompressor_tag; typedef struct tinfl_decompressor_tag tinfl_decompressor; @@ -427,15 +428,15 @@ typedef struct } tinfl_huff_table; #if MINIZ_HAS_64BIT_REGISTERS - #define TINFL_USE_64BIT_BITBUF 1 +#define TINFL_USE_64BIT_BITBUF 1 #endif #if TINFL_USE_64BIT_BITBUF - typedef mz_uint64 tinfl_bit_buf_t; - #define TINFL_BITBUF_SIZE (64) +typedef mz_uint64 tinfl_bit_buf_t; +#define TINFL_BITBUF_SIZE (64) #else - typedef mz_uint32 tinfl_bit_buf_t; - #define TINFL_BITBUF_SIZE (32) +typedef mz_uint32 tinfl_bit_buf_t; +#define TINFL_BITBUF_SIZE (32) #endif struct tinfl_decompressor_tag @@ -470,14 +471,14 @@ enum // The low 12 bits are reserved to control the max # of hash probes per dictionary lookup (see TDEFL_MAX_PROBES_MASK). enum { - TDEFL_WRITE_ZLIB_HEADER = 0x01000, - TDEFL_COMPUTE_ADLER32 = 0x02000, - TDEFL_GREEDY_PARSING_FLAG = 0x04000, + TDEFL_WRITE_ZLIB_HEADER = 0x01000, + TDEFL_COMPUTE_ADLER32 = 0x02000, + TDEFL_GREEDY_PARSING_FLAG = 0x04000, TDEFL_NONDETERMINISTIC_PARSING_FLAG = 0x08000, - TDEFL_RLE_MATCHES = 0x10000, - TDEFL_FILTER_MATCHES = 0x20000, - TDEFL_FORCE_ALL_STATIC_BLOCKS = 0x40000, - TDEFL_FORCE_ALL_RAW_BLOCKS = 0x80000 + TDEFL_RLE_MATCHES = 0x10000, + TDEFL_FILTER_MATCHES = 0x20000, + TDEFL_FORCE_ALL_STATIC_BLOCKS = 0x40000, + TDEFL_FORCE_ALL_RAW_BLOCKS = 0x80000 }; // High level compression functions: @@ -497,7 +498,7 @@ size_t tdefl_compress_mem_to_mem(void *pOut_buf, size_t out_buf_len, const void // Compresses an image to a compressed PNG file in memory. // On entry: -// pImage, w, h, and num_chans describe the image to compress. num_chans may be 1, 2, 3, or 4. +// pImage, w, h, and num_chans describe the image to compress. num_chans may be 1, 2, 3, or 4. // The image pitch in bytes per scanline will be w*num_chans. The leftmost pixel on the top scanline is stored first in memory. // level may range from [0,10], use MZ_NO_COMPRESSION, MZ_BEST_SPEED, MZ_BEST_COMPRESSION, etc. or a decent default is MZ_DEFAULT_LEVEL // If flip is true, the image will be flipped on the Y axis (useful for OpenGL apps). @@ -509,7 +510,7 @@ void *tdefl_write_image_to_png_file_in_memory_ex(const void *pImage, int w, int void *tdefl_write_image_to_png_file_in_memory(const void *pImage, int w, int h, int num_chans, size_t *pLen_out); // Output stream interface. The compressor uses this interface to write compressed data. It'll typically be called TDEFL_OUT_BUF_SIZE at a time. -typedef mz_bool (*tdefl_put_buf_func_ptr)(const void* pBuf, int len, void *pUser); +typedef mz_bool(*tdefl_put_buf_func_ptr)(const void *pBuf, int len, void *pUser); // tdefl_compress_mem_to_output() compresses a block to an output stream. The above helpers use this function internally. mz_bool tdefl_compress_mem_to_output(const void *pBuf, size_t buf_len, tdefl_put_buf_func_ptr pPut_buf_func, void *pPut_buf_user, int flags); @@ -518,9 +519,9 @@ enum { TDEFL_MAX_HUFF_TABLES = 3, TDEFL_MAX_HUFF_SYMBOLS_0 = 288, TDEFL_MAX_HUFF // TDEFL_OUT_BUF_SIZE MUST be large enough to hold a single entire compressed output block (using static/fixed Huffman codes). #if TDEFL_LESS_MEMORY -enum { TDEFL_LZ_CODE_BUF_SIZE = 24 * 1024, TDEFL_OUT_BUF_SIZE = (TDEFL_LZ_CODE_BUF_SIZE * 13 ) / 10, TDEFL_MAX_HUFF_SYMBOLS = 288, TDEFL_LZ_HASH_BITS = 12, TDEFL_LEVEL1_HASH_SIZE_MASK = 4095, TDEFL_LZ_HASH_SHIFT = (TDEFL_LZ_HASH_BITS + 2) / 3, TDEFL_LZ_HASH_SIZE = 1 << TDEFL_LZ_HASH_BITS }; +enum { TDEFL_LZ_CODE_BUF_SIZE = 24 * 1024, TDEFL_OUT_BUF_SIZE = (TDEFL_LZ_CODE_BUF_SIZE * 13) / 10, TDEFL_MAX_HUFF_SYMBOLS = 288, TDEFL_LZ_HASH_BITS = 12, TDEFL_LEVEL1_HASH_SIZE_MASK = 4095, TDEFL_LZ_HASH_SHIFT = (TDEFL_LZ_HASH_BITS + 2) / 3, TDEFL_LZ_HASH_SIZE = 1 << TDEFL_LZ_HASH_BITS }; #else -enum { TDEFL_LZ_CODE_BUF_SIZE = 64 * 1024, TDEFL_OUT_BUF_SIZE = (TDEFL_LZ_CODE_BUF_SIZE * 13 ) / 10, TDEFL_MAX_HUFF_SYMBOLS = 288, TDEFL_LZ_HASH_BITS = 15, TDEFL_LEVEL1_HASH_SIZE_MASK = 4095, TDEFL_LZ_HASH_SHIFT = (TDEFL_LZ_HASH_BITS + 2) / 3, TDEFL_LZ_HASH_SIZE = 1 << TDEFL_LZ_HASH_BITS }; +enum { TDEFL_LZ_CODE_BUF_SIZE = 64 * 1024, TDEFL_OUT_BUF_SIZE = (TDEFL_LZ_CODE_BUF_SIZE * 13) / 10, TDEFL_MAX_HUFF_SYMBOLS = 288, TDEFL_LZ_HASH_BITS = 15, TDEFL_LEVEL1_HASH_SIZE_MASK = 4095, TDEFL_LZ_HASH_SHIFT = (TDEFL_LZ_HASH_BITS + 2) / 3, TDEFL_LZ_HASH_SIZE = 1 << TDEFL_LZ_HASH_BITS }; #endif // The low-level tdefl functions below may be used directly if the above helper functions aren't flexible enough. The low-level functions don't make any heap allocations, unlike the above helper functions. @@ -606,7 +607,8 @@ mz_uint tdefl_create_comp_flags_from_zip_params(int level, int window_bits, int #include #include -namespace buminiz { +namespace buminiz +{ typedef unsigned char mz_validate_uint16[sizeof(mz_uint16)==2 ? 1 : -1]; typedef unsigned char mz_validate_uint32[sizeof(mz_uint32)==4 ? 1 : -1]; @@ -615,13 +617,13 @@ typedef unsigned char mz_validate_uint64[sizeof(mz_uint64)==8 ? 1 : -1]; #define MZ_ASSERT(x) assert(x) #ifdef MINIZ_NO_MALLOC - #define MZ_MALLOC(x) NULL - #define MZ_FREE(x) (void)x, ((void)0) - #define MZ_REALLOC(p, x) NULL +#define MZ_MALLOC(x) NULL +#define MZ_FREE(x) (void)x, ((void)0) +#define MZ_REALLOC(p, x) NULL #else - #define MZ_MALLOC(x) malloc(x) - #define MZ_FREE(x) free(x) - #define MZ_REALLOC(p, x) realloc(p, x) +#define MZ_MALLOC(x) malloc(x) +#define MZ_FREE(x) free(x) +#define MZ_REALLOC(p, x) realloc(p, x) #endif #define MZ_MAX(a,b) (((a)>(b))?(a):(b)) @@ -629,19 +631,19 @@ typedef unsigned char mz_validate_uint64[sizeof(mz_uint64)==8 ? 1 : -1]; #define MZ_CLEAR_OBJ(obj) memset(&(obj), 0, sizeof(obj)) #if MINIZ_USE_UNALIGNED_LOADS_AND_STORES && MINIZ_LITTLE_ENDIAN - #define MZ_READ_LE16(p) *((const mz_uint16 *)(p)) - #define MZ_READ_LE32(p) *((const mz_uint32 *)(p)) +#define MZ_READ_LE16(p) *((const mz_uint16 *)(p)) +#define MZ_READ_LE32(p) *((const mz_uint32 *)(p)) #else - #define MZ_READ_LE16(p) ((mz_uint32)(((const mz_uint8 *)(p))[0]) | ((mz_uint32)(((const mz_uint8 *)(p))[1]) << 8U)) - #define MZ_READ_LE32(p) ((mz_uint32)(((const mz_uint8 *)(p))[0]) | ((mz_uint32)(((const mz_uint8 *)(p))[1]) << 8U) | ((mz_uint32)(((const mz_uint8 *)(p))[2]) << 16U) | ((mz_uint32)(((const mz_uint8 *)(p))[3]) << 24U)) +#define MZ_READ_LE16(p) ((mz_uint32)(((const mz_uint8 *)(p))[0]) | ((mz_uint32)(((const mz_uint8 *)(p))[1]) << 8U)) +#define MZ_READ_LE32(p) ((mz_uint32)(((const mz_uint8 *)(p))[0]) | ((mz_uint32)(((const mz_uint8 *)(p))[1]) << 8U) | ((mz_uint32)(((const mz_uint8 *)(p))[2]) << 16U) | ((mz_uint32)(((const mz_uint8 *)(p))[3]) << 24U)) #endif #ifdef _MSC_VER - #define MZ_FORCEINLINE __forceinline +#define MZ_FORCEINLINE __forceinline #elif defined(__GNUC__) - #define MZ_FORCEINLINE inline __attribute__((__always_inline__)) +#define MZ_FORCEINLINE inline __attribute__((__always_inline__)) #else - #define MZ_FORCEINLINE inline +#define MZ_FORCEINLINE inline #endif // ------------------- zlib-style API's @@ -655,7 +657,7 @@ mz_ulong mz_adler32(mz_ulong adler, const unsigned char *ptr, size_t buf_len) s1 += ptr[0], s2 += s1; s1 += ptr[1], s2 += s1; s1 += ptr[2], s2 += s1; s1 += ptr[3], s2 += s1; s1 += ptr[4], s2 += s1; s1 += ptr[5], s2 += s1; s1 += ptr[6], s2 += s1; s1 += ptr[7], s2 += s1; } - for ( ; i < block_len; ++i) s1 += *ptr++, s2 += s1; + for (; i < block_len; ++i) s1 += *ptr++, s2 += s1; s1 %= 65521U, s2 %= 65521U; buf_len -= block_len; block_len = 5552; } return (s2 << 16) + s1; @@ -716,8 +718,7 @@ int mz_deflateInit2(mz_streamp pStream, int level, int method, int window_bits, pStream->state = (struct mz_internal_state *)pComp; - if (tdefl_init(pComp, NULL, NULL, comp_flags) != TDEFL_STATUS_OKAY) - { + if (tdefl_init(pComp, NULL, NULL, comp_flags) != TDEFL_STATUS_OKAY) { mz_deflateEnd(pStream); return MZ_PARAM_ERROR; } @@ -729,7 +730,7 @@ int mz_deflateReset(mz_streamp pStream) { if ((!pStream) || (!pStream->state) || (!pStream->zalloc) || (!pStream->zfree)) return MZ_STREAM_ERROR; pStream->total_in = pStream->total_out = 0; - tdefl_init((tdefl_compressor*)pStream->state, NULL, NULL, ((tdefl_compressor*)pStream->state)->m_flags); + tdefl_init((tdefl_compressor *)pStream->state, NULL, NULL, ((tdefl_compressor *)pStream->state)->m_flags); return MZ_OK; } @@ -744,36 +745,32 @@ int mz_deflate(mz_streamp pStream, int flush) if (flush == MZ_PARTIAL_FLUSH) flush = MZ_SYNC_FLUSH; - if (((tdefl_compressor*)pStream->state)->m_prev_return_status == TDEFL_STATUS_DONE) + if (((tdefl_compressor *)pStream->state)->m_prev_return_status == TDEFL_STATUS_DONE) return (flush == MZ_FINISH) ? MZ_STREAM_END : MZ_BUF_ERROR; orig_total_in = pStream->total_in; orig_total_out = pStream->total_out; - for ( ; ; ) - { + for (; ; ) { tdefl_status defl_status; in_bytes = pStream->avail_in; out_bytes = pStream->avail_out; - defl_status = tdefl_compress((tdefl_compressor*)pStream->state, pStream->next_in, &in_bytes, pStream->next_out, &out_bytes, (tdefl_flush)flush); + defl_status = tdefl_compress((tdefl_compressor *)pStream->state, pStream->next_in, &in_bytes, pStream->next_out, &out_bytes, (tdefl_flush)flush); pStream->next_in += (mz_uint)in_bytes; pStream->avail_in -= (mz_uint)in_bytes; - pStream->total_in += (mz_uint)in_bytes; pStream->adler = tdefl_get_adler32((tdefl_compressor*)pStream->state); + pStream->total_in += (mz_uint)in_bytes; pStream->adler = tdefl_get_adler32((tdefl_compressor *)pStream->state); pStream->next_out += (mz_uint)out_bytes; pStream->avail_out -= (mz_uint)out_bytes; pStream->total_out += (mz_uint)out_bytes; - if (defl_status < 0) - { + if (defl_status < 0) { mz_status = MZ_STREAM_ERROR; break; } - else if (defl_status == TDEFL_STATUS_DONE) - { + else if (defl_status == TDEFL_STATUS_DONE) { mz_status = MZ_STREAM_END; break; } else if (!pStream->avail_out) break; - else if ((!pStream->avail_in) && (flush != MZ_FINISH)) - { + else if ((!pStream->avail_in) && (flush != MZ_FINISH)) { if ((flush) || (pStream->total_in != orig_total_in) || (pStream->total_out != orig_total_out)) break; return MZ_BUF_ERROR; // Can't make forward progress without some input. @@ -785,8 +782,7 @@ int mz_deflate(mz_streamp pStream, int flush) int mz_deflateEnd(mz_streamp pStream) { if (!pStream) return MZ_STREAM_ERROR; - if (pStream->state) - { + if (pStream->state) { pStream->zfree(pStream->opaque, pStream->state); pStream->state = NULL; } @@ -799,10 +795,10 @@ mz_ulong mz_deflateBound(mz_streamp pStream, mz_ulong source_len) // This is really over conservative. (And lame, but it's actually pretty tricky to compute a true upper bound given the way tdefl's blocking works.) mz_uint64 a = 128ULL + (source_len * 110ULL) / 100ULL; mz_uint64 b = 128ULL + (mz_uint64)source_len + ((source_len / (31 * 1024)) + 1ULL) * 5ULL; - + mz_uint64 t = MZ_MAX(a, b); if (((mz_ulong)t) != t) - t = (mz_ulong)(-1); + t = (mz_ulong)(-1); return (mz_ulong)t; } @@ -825,8 +821,7 @@ int mz_compress2(unsigned char *pDest, mz_ulong *pDest_len, const unsigned char if (status != MZ_OK) return status; status = mz_deflate(&stream, MZ_FINISH); - if (status != MZ_STREAM_END) - { + if (status != MZ_STREAM_END) { mz_deflateEnd(&stream); return (status == MZ_OK) ? MZ_BUF_ERROR : status; } @@ -868,7 +863,7 @@ int mz_inflateInit2(mz_streamp pStream, int window_bits) if (!pStream->zalloc) pStream->zalloc = def_alloc_func; if (!pStream->zfree) pStream->zfree = def_free_func; - pDecomp = (inflate_state*)pStream->zalloc(pStream->opaque, 1, sizeof(inflate_state)); + pDecomp = (inflate_state *)pStream->zalloc(pStream->opaque, 1, sizeof(inflate_state)); if (!pDecomp) return MZ_MEM_ERROR; pStream->state = (struct mz_internal_state *)pDecomp; @@ -886,12 +881,12 @@ int mz_inflateInit2(mz_streamp pStream, int window_bits) int mz_inflateInit(mz_streamp pStream) { - return mz_inflateInit2(pStream, MZ_DEFAULT_WINDOW_BITS); + return mz_inflateInit2(pStream, MZ_DEFAULT_WINDOW_BITS); } int mz_inflate2(mz_streamp pStream, int flush, int adler32_checking) { - inflate_state* pState; + inflate_state *pState; mz_uint n, first_call, decomp_flags = adler32_checking ? TINFL_FLAG_COMPUTE_ADLER32 : 0; size_t in_bytes, out_bytes, orig_avail_in; tinfl_status status; @@ -900,7 +895,7 @@ int mz_inflate2(mz_streamp pStream, int flush, int adler32_checking) if (flush == MZ_PARTIAL_FLUSH) flush = MZ_SYNC_FLUSH; if ((flush) && (flush != MZ_SYNC_FLUSH) && (flush != MZ_FINISH)) return MZ_STREAM_ERROR; - pState = (inflate_state*)pStream->state; + pState = (inflate_state *)pStream->state; if (pState->m_window_bits > 0) decomp_flags |= TINFL_FLAG_PARSE_ZLIB_HEADER; orig_avail_in = pStream->avail_in; @@ -910,8 +905,7 @@ int mz_inflate2(mz_streamp pStream, int flush, int adler32_checking) if (pState->m_has_flushed && (flush != MZ_FINISH)) return MZ_STREAM_ERROR; pState->m_has_flushed |= (flush == MZ_FINISH); - if ((flush == MZ_FINISH) && (first_call)) - { + if ((flush == MZ_FINISH) && (first_call)) { // MZ_FINISH on the first call implies that the input and output buffers are large enough to hold the entire compressed/decompressed file. decomp_flags |= TINFL_FLAG_USING_NON_WRAPPING_OUTPUT_BUF; in_bytes = pStream->avail_in; out_bytes = pStream->avail_out; @@ -923,8 +917,7 @@ int mz_inflate2(mz_streamp pStream, int flush, int adler32_checking) if (status < 0) return MZ_DATA_ERROR; - else if (status != TINFL_STATUS_DONE) - { + else if (status != TINFL_STATUS_DONE) { pState->m_last_status = TINFL_STATUS_FAILED; return MZ_BUF_ERROR; } @@ -933,8 +926,7 @@ int mz_inflate2(mz_streamp pStream, int flush, int adler32_checking) // flush != MZ_FINISH then we must assume there's more input. if (flush != MZ_FINISH) decomp_flags |= TINFL_FLAG_HAS_MORE_INPUT; - if (pState->m_dict_avail) - { + if (pState->m_dict_avail) { n = MZ_MIN(pState->m_dict_avail, pStream->avail_out); memcpy(pStream->next_out, pState->m_dict + pState->m_dict_ofs, n); pStream->next_out += n; pStream->avail_out -= n; pStream->total_out += n; @@ -942,8 +934,7 @@ int mz_inflate2(mz_streamp pStream, int flush, int adler32_checking) return ((pState->m_last_status == TINFL_STATUS_DONE) && (!pState->m_dict_avail)) ? MZ_STREAM_END : MZ_OK; } - for ( ; ; ) - { + for (; ; ) { in_bytes = pStream->avail_in; out_bytes = TINFL_LZ_DICT_SIZE - pState->m_dict_ofs; @@ -961,17 +952,16 @@ int mz_inflate2(mz_streamp pStream, int flush, int adler32_checking) pState->m_dict_avail -= n; pState->m_dict_ofs = (pState->m_dict_ofs + n) & (TINFL_LZ_DICT_SIZE - 1); if (status < 0) - return MZ_DATA_ERROR; // Stream is corrupted (there could be some uncompressed data left in the output dictionary - oh well). + return MZ_DATA_ERROR; // Stream is corrupted (there could be some uncompressed data left in the output dictionary - oh well). else if ((status == TINFL_STATUS_NEEDS_MORE_INPUT) && (!orig_avail_in)) return MZ_BUF_ERROR; // Signal caller that we can't make forward progress without supplying more input or by setting flush to MZ_FINISH. - else if (flush == MZ_FINISH) - { + else if (flush == MZ_FINISH) { // The output buffer MUST be large to hold the remaining uncompressed data when flush==MZ_FINISH. - if (status == TINFL_STATUS_DONE) - return pState->m_dict_avail ? MZ_BUF_ERROR : MZ_STREAM_END; - // status here must be TINFL_STATUS_HAS_MORE_OUTPUT, which means there's at least 1 more byte on the way. If there's no more room left in the output buffer then something is wrong. - else if (!pStream->avail_out) - return MZ_BUF_ERROR; + if (status == TINFL_STATUS_DONE) + return pState->m_dict_avail ? MZ_BUF_ERROR : MZ_STREAM_END; + // status here must be TINFL_STATUS_HAS_MORE_OUTPUT, which means there's at least 1 more byte on the way. If there's no more room left in the output buffer then something is wrong. + else if (!pStream->avail_out) + return MZ_BUF_ERROR; } else if ((status == TINFL_STATUS_DONE) || (!pStream->avail_in) || (!pStream->avail_out) || (pState->m_dict_avail)) break; @@ -982,15 +972,14 @@ int mz_inflate2(mz_streamp pStream, int flush, int adler32_checking) int mz_inflate(mz_streamp pStream, int flush) { - return mz_inflate2(pStream, flush, MZ_TRUE); + return mz_inflate2(pStream, flush, MZ_TRUE); } int mz_inflateEnd(mz_streamp pStream) { if (!pStream) return MZ_STREAM_ERROR; - if (pStream->state) - { + if (pStream->state) { pStream->zfree(pStream->opaque, pStream->state); pStream->state = NULL; } @@ -1016,8 +1005,7 @@ int mz_uncompress(unsigned char *pDest, mz_ulong *pDest_len, const unsigned char return status; status = mz_inflate(&stream, MZ_FINISH); - if (status != MZ_STREAM_END) - { + if (status != MZ_STREAM_END) { mz_inflateEnd(&stream); return ((status == MZ_BUF_ERROR) && (!stream.avail_in)) ? MZ_DATA_ERROR : status; } @@ -1112,9 +1100,9 @@ const char *mz_error(int err) tinfl_status tinfl_decompress(tinfl_decompressor *r, const mz_uint8 *pIn_buf_next, size_t *pIn_buf_size, mz_uint8 *pOut_buf_start, mz_uint8 *pOut_buf_next, size_t *pOut_buf_size, const mz_uint32 decomp_flags) { static const int s_length_base[31] = { 3,4,5,6,7,8,9,10,11,13, 15,17,19,23,27,31,35,43,51,59, 67,83,99,115,131,163,195,227,258,0,0 }; - static const int s_length_extra[31]= { 0,0,0,0,0,0,0,0,1,1,1,1,2,2,2,2,3,3,3,3,4,4,4,4,5,5,5,5,0,0,0 }; - static const int s_dist_base[32] = { 1,2,3,4,5,7,9,13,17,25,33,49,65,97,129,193, 257,385,513,769,1025,1537,2049,3073,4097,6145,8193,12289,16385,24577,0,0}; - static const int s_dist_extra[32] = { 0,0,0,0,1,1,2,2,3,3,4,4,5,5,6,6,7,7,8,8,9,9,10,10,11,11,12,12,13,13}; + static const int s_length_extra[31] = { 0,0,0,0,0,0,0,0,1,1,1,1,2,2,2,2,3,3,3,3,4,4,4,4,5,5,5,5,0,0,0 }; + static const int s_dist_base[32] = { 1,2,3,4,5,7,9,13,17,25,33,49,65,97,129,193, 257,385,513,769,1025,1537,2049,3073,4097,6145,8193,12289,16385,24577,0,0 }; + static const int s_dist_extra[32] = { 0,0,0,0,1,1,2,2,3,3,4,4,5,5,6,6,7,7,8,8,9,9,10,10,11,11,12,12,13,13 }; static const mz_uint8 s_length_dezigzag[19] = { 16,17,18,0,8,7,9,6,10,5,11,4,12,3,13,2,14,1,15 }; static const int s_min_table_sizes[3] = { 257, 1, 4 }; @@ -1129,41 +1117,33 @@ tinfl_status tinfl_decompress(tinfl_decompressor *r, const mz_uint8 *pIn_buf_nex num_bits = r->m_num_bits; bit_buf = r->m_bit_buf; dist = r->m_dist; counter = r->m_counter; num_extra = r->m_num_extra; dist_from_out_buf_start = r->m_dist_from_out_buf_start; TINFL_CR_BEGIN - bit_buf = num_bits = dist = counter = num_extra = r->m_zhdr0 = r->m_zhdr1 = 0; r->m_z_adler32 = r->m_check_adler32 = 1; - if (decomp_flags & TINFL_FLAG_PARSE_ZLIB_HEADER) - { + bit_buf = num_bits = dist = counter = num_extra = r->m_zhdr0 = r->m_zhdr1 = 0; r->m_z_adler32 = r->m_check_adler32 = 1; + if (decomp_flags & TINFL_FLAG_PARSE_ZLIB_HEADER) { TINFL_GET_BYTE(1, r->m_zhdr0); TINFL_GET_BYTE(2, r->m_zhdr1); counter = (((r->m_zhdr0 * 256 + r->m_zhdr1) % 31 != 0) || (r->m_zhdr1 & 32) || ((r->m_zhdr0 & 15) != 8)); if (!(decomp_flags & TINFL_FLAG_USING_NON_WRAPPING_OUTPUT_BUF)) counter |= (((1U << (8U + (r->m_zhdr0 >> 4))) > 32768U) || ((out_buf_size_mask + 1) < (size_t)(1ULL << (8U + (r->m_zhdr0 >> 4))))); if (counter) { TINFL_CR_RETURN_FOREVER(36, TINFL_STATUS_FAILED); } } - do - { + do { TINFL_GET_BITS(3, r->m_final, 3); r->m_type = r->m_final >> 1; - if (r->m_type == 0) - { + if (r->m_type == 0) { TINFL_SKIP_BITS(5, num_bits & 7); for (counter = 0; counter < 4; ++counter) { if (num_bits) TINFL_GET_BITS(6, r->m_raw_header[counter], 8); else TINFL_GET_BYTE(7, r->m_raw_header[counter]); } if ((counter = (r->m_raw_header[0] | (r->m_raw_header[1] << 8))) != (mz_uint)(0xFFFF ^ (r->m_raw_header[2] | (r->m_raw_header[3] << 8)))) { TINFL_CR_RETURN_FOREVER(39, TINFL_STATUS_FAILED); } - while ((counter) && (num_bits)) - { + while ((counter) && (num_bits)) { TINFL_GET_BITS(51, dist, 8); while (pOut_buf_cur >= pOut_buf_end) { TINFL_CR_RETURN(52, TINFL_STATUS_HAS_MORE_OUTPUT); } *pOut_buf_cur++ = (mz_uint8)dist; counter--; } - while (counter) - { + while (counter) { size_t n; while (pOut_buf_cur >= pOut_buf_end) { TINFL_CR_RETURN(9, TINFL_STATUS_HAS_MORE_OUTPUT); } - while (pIn_buf_cur >= pIn_buf_end) - { - if (decomp_flags & TINFL_FLAG_HAS_MORE_INPUT) - { + while (pIn_buf_cur >= pIn_buf_end) { + if (decomp_flags & TINFL_FLAG_HAS_MORE_INPUT) { TINFL_CR_RETURN(38, TINFL_STATUS_NEEDS_MORE_INPUT); } - else - { + else { TINFL_CR_RETURN_FOREVER(40, TINFL_STATUS_FAILED); } } @@ -1171,94 +1151,79 @@ tinfl_status tinfl_decompress(tinfl_decompressor *r, const mz_uint8 *pIn_buf_nex TINFL_MEMCPY(pOut_buf_cur, pIn_buf_cur, n); pIn_buf_cur += n; pOut_buf_cur += n; counter -= (mz_uint)n; } } - else if (r->m_type == 3) - { + else if (r->m_type == 3) { TINFL_CR_RETURN_FOREVER(10, TINFL_STATUS_FAILED); } - else - { - if (r->m_type == 1) - { + else { + if (r->m_type == 1) { mz_uint8 *p = r->m_tables[0].m_code_size; mz_uint i; r->m_table_sizes[0] = 288; r->m_table_sizes[1] = 32; TINFL_MEMSET(r->m_tables[1].m_code_size, 5, 32); - for ( i = 0; i <= 143; ++i) { + for (i = 0; i <= 143; ++i) { *p++ = 8; } - for ( ; i <= 255; ++i) { + for (; i <= 255; ++i) { *p++ = 9; } - for ( ; i <= 279; ++i) { + for (; i <= 279; ++i) { *p++ = 7; } - for ( ; i <= 287; ++i) { + for (; i <= 287; ++i) { *p++ = 8; } } - else - { + else { for (counter = 0; counter < 3; counter++) { TINFL_GET_BITS(11, r->m_table_sizes[counter], "\05\05\04"[counter]); r->m_table_sizes[counter] += s_min_table_sizes[counter]; } MZ_CLEAR_OBJ(r->m_tables[2].m_code_size); for (counter = 0; counter < r->m_table_sizes[2]; counter++) { mz_uint s; TINFL_GET_BITS(14, s, 3); r->m_tables[2].m_code_size[s_length_dezigzag[counter]] = (mz_uint8)s; } r->m_table_sizes[2] = 19; } - for ( ; (int)r->m_type >= 0; r->m_type--) - { + for (; (int)r->m_type >= 0; r->m_type--) { int tree_next, tree_cur; tinfl_huff_table *pTable; mz_uint i, j, used_syms, total, sym_index, next_code[17], total_syms[16]; pTable = &r->m_tables[r->m_type]; MZ_CLEAR_OBJ(total_syms); MZ_CLEAR_OBJ(pTable->m_look_up); MZ_CLEAR_OBJ(pTable->m_tree); for (i = 0; i < r->m_table_sizes[r->m_type]; ++i) total_syms[pTable->m_code_size[i]]++; used_syms = 0, total = 0; next_code[0] = next_code[1] = 0; for (i = 1; i <= 15; ++i) { used_syms += total_syms[i]; next_code[i + 1] = (total = ((total + total_syms[i]) << 1)); } - if ((65536 != total) && (used_syms > 1)) - { + if ((65536 != total) && (used_syms > 1)) { TINFL_CR_RETURN_FOREVER(35, TINFL_STATUS_FAILED); } - for (tree_next = -1, sym_index = 0; sym_index < r->m_table_sizes[r->m_type]; ++sym_index) - { + for (tree_next = -1, sym_index = 0; sym_index < r->m_table_sizes[r->m_type]; ++sym_index) { mz_uint rev_code = 0, l, cur_code, code_size = pTable->m_code_size[sym_index]; if (!code_size) continue; cur_code = next_code[code_size]++; for (l = code_size; l > 0; l--, cur_code >>= 1) rev_code = (rev_code << 1) | (cur_code & 1); if (code_size <= TINFL_FAST_LOOKUP_BITS) { mz_int16 k = (mz_int16)((code_size << 9) | sym_index); while (rev_code < TINFL_FAST_LOOKUP_SIZE) { pTable->m_look_up[rev_code] = k; rev_code += (1 << code_size); } continue; } if (0 == (tree_cur = pTable->m_look_up[rev_code & (TINFL_FAST_LOOKUP_SIZE - 1)])) { pTable->m_look_up[rev_code & (TINFL_FAST_LOOKUP_SIZE - 1)] = (mz_int16)tree_next; tree_cur = tree_next; tree_next -= 2; } rev_code >>= (TINFL_FAST_LOOKUP_BITS - 1); - for (j = code_size; j > (TINFL_FAST_LOOKUP_BITS + 1); j--) - { + for (j = code_size; j >(TINFL_FAST_LOOKUP_BITS + 1); j--) { tree_cur -= ((rev_code >>= 1) & 1); - if (!pTable->m_tree[-tree_cur - 1]) { pTable->m_tree[-tree_cur - 1] = (mz_int16)tree_next; tree_cur = tree_next; tree_next -= 2; } else tree_cur = pTable->m_tree[-tree_cur - 1]; + if (!pTable->m_tree[-tree_cur - 1]) { pTable->m_tree[-tree_cur - 1] = (mz_int16)tree_next; tree_cur = tree_next; tree_next -= 2; } + else tree_cur = pTable->m_tree[-tree_cur - 1]; } tree_cur -= ((rev_code >>= 1) & 1); pTable->m_tree[-tree_cur - 1] = (mz_int16)sym_index; } - if (r->m_type == 2) - { - for (counter = 0; counter < (r->m_table_sizes[0] + r->m_table_sizes[1]); ) - { + if (r->m_type == 2) { + for (counter = 0; counter < (r->m_table_sizes[0] + r->m_table_sizes[1]); ) { mz_uint s; TINFL_HUFF_DECODE(16, dist, &r->m_tables[2]); if (dist < 16) { r->m_len_codes[counter++] = (mz_uint8)dist; continue; } - if ((dist == 16) && (!counter)) - { + if ((dist == 16) && (!counter)) { TINFL_CR_RETURN_FOREVER(17, TINFL_STATUS_FAILED); } num_extra = "\02\03\07"[dist - 16]; TINFL_GET_BITS(18, s, num_extra); s += "\03\03\013"[dist - 16]; TINFL_MEMSET(r->m_len_codes + counter, (dist == 16) ? r->m_len_codes[counter - 1] : 0, s); counter += s; } - if ((r->m_table_sizes[0] + r->m_table_sizes[1]) != counter) - { + if ((r->m_table_sizes[0] + r->m_table_sizes[1]) != counter) { TINFL_CR_RETURN_FOREVER(21, TINFL_STATUS_FAILED); } TINFL_MEMCPY(r->m_tables[0].m_code_size, r->m_len_codes, r->m_table_sizes[0]); TINFL_MEMCPY(r->m_tables[1].m_code_size, r->m_len_codes + r->m_table_sizes[0], r->m_table_sizes[1]); } } - for ( ; ; ) - { + for (; ; ) { mz_uint8 *pSrc; - for ( ; ; ) - { - if (((pIn_buf_end - pIn_buf_cur) < 4) || ((pOut_buf_end - pOut_buf_cur) < 2)) - { + for (; ; ) { + if (((pIn_buf_end - pIn_buf_cur) < 4) || ((pOut_buf_end - pOut_buf_cur) < 2)) { TINFL_HUFF_DECODE(23, counter, &r->m_tables[0]); if (counter >= 256) break; while (pOut_buf_cur >= pOut_buf_end) { TINFL_CR_RETURN(24, TINFL_STATUS_HAS_MORE_OUTPUT); } *pOut_buf_cur++ = (mz_uint8)counter; } - else - { + else { int sym2; mz_uint code_len; #if TINFL_USE_64BIT_BITBUF if (num_bits < 30) { bit_buf |= (((tinfl_bit_buf_t)MZ_READ_LE32(pIn_buf_cur)) << num_bits); pIn_buf_cur += 4; num_bits += 32; } @@ -1267,8 +1232,7 @@ tinfl_status tinfl_decompress(tinfl_decompressor *r, const mz_uint8 *pIn_buf_nex #endif if ((sym2 = r->m_tables[0].m_look_up[bit_buf & (TINFL_FAST_LOOKUP_SIZE - 1)]) >= 0) code_len = sym2 >> 9; - else - { + else { code_len = TINFL_FAST_LOOKUP_BITS; do { sym2 = r->m_tables[0].m_tree[~sym2 + ((bit_buf >> code_len++) & 1)]; } while (sym2 < 0); } counter = sym2; bit_buf >>= code_len; num_bits -= code_len; @@ -1280,15 +1244,13 @@ tinfl_status tinfl_decompress(tinfl_decompressor *r, const mz_uint8 *pIn_buf_nex #endif if ((sym2 = r->m_tables[0].m_look_up[bit_buf & (TINFL_FAST_LOOKUP_SIZE - 1)]) >= 0) code_len = sym2 >> 9; - else - { + else { code_len = TINFL_FAST_LOOKUP_BITS; do { sym2 = r->m_tables[0].m_tree[~sym2 + ((bit_buf >> code_len++) & 1)]; } while (sym2 < 0); } bit_buf >>= code_len; num_bits -= code_len; pOut_buf_cur[0] = (mz_uint8)counter; - if (sym2 & 256) - { + if (sym2 & 256) { pOut_buf_cur++; counter = sym2; break; @@ -1307,36 +1269,29 @@ tinfl_status tinfl_decompress(tinfl_decompressor *r, const mz_uint8 *pIn_buf_nex if (num_extra) { mz_uint extra_bits; TINFL_GET_BITS(27, extra_bits, num_extra); dist += extra_bits; } dist_from_out_buf_start = pOut_buf_cur - pOut_buf_start; - if ((dist > dist_from_out_buf_start) && (decomp_flags & TINFL_FLAG_USING_NON_WRAPPING_OUTPUT_BUF)) - { + if ((dist > dist_from_out_buf_start) && (decomp_flags & TINFL_FLAG_USING_NON_WRAPPING_OUTPUT_BUF)) { TINFL_CR_RETURN_FOREVER(37, TINFL_STATUS_FAILED); } pSrc = pOut_buf_start + ((dist_from_out_buf_start - dist) & out_buf_size_mask); - if ((MZ_MAX(pOut_buf_cur, pSrc) + counter) > pOut_buf_end) - { - while (counter--) - { + if ((MZ_MAX(pOut_buf_cur, pSrc) + counter) > pOut_buf_end) { + while (counter--) { while (pOut_buf_cur >= pOut_buf_end) { TINFL_CR_RETURN(53, TINFL_STATUS_HAS_MORE_OUTPUT); } *pOut_buf_cur++ = pOut_buf_start[(dist_from_out_buf_start++ - dist) & out_buf_size_mask]; } continue; } #if MINIZ_USE_UNALIGNED_LOADS_AND_STORES - else if ((counter >= 9) && (counter <= dist)) - { + else if ((counter >= 9) && (counter <= dist)) { const mz_uint8 *pSrc_end = pSrc + (counter & ~7); - do - { + do { ((mz_uint32 *)pOut_buf_cur)[0] = ((const mz_uint32 *)pSrc)[0]; ((mz_uint32 *)pOut_buf_cur)[1] = ((const mz_uint32 *)pSrc)[1]; pOut_buf_cur += 8; } while ((pSrc += 8) < pSrc_end); - if ((counter &= 7) < 3) - { - if (counter) - { + if ((counter &= 7) < 3) { + if (counter) { pOut_buf_cur[0] = pSrc[0]; if (counter > 1) pOut_buf_cur[1] = pSrc[1]; @@ -1346,15 +1301,13 @@ tinfl_status tinfl_decompress(tinfl_decompressor *r, const mz_uint8 *pIn_buf_nex } } #endif - do - { + do { pOut_buf_cur[0] = pSrc[0]; pOut_buf_cur[1] = pSrc[1]; pOut_buf_cur[2] = pSrc[2]; pOut_buf_cur += 3; pSrc += 3; } while ((int)(counter -= 3) > 2); - if ((int)counter > 0) - { + if ((int)counter > 0) { pOut_buf_cur[0] = pSrc[0]; if ((int)counter > 1) pOut_buf_cur[1] = pSrc[1]; @@ -1363,34 +1316,30 @@ tinfl_status tinfl_decompress(tinfl_decompressor *r, const mz_uint8 *pIn_buf_nex } } } while (!(r->m_final & 1)); - if (decomp_flags & TINFL_FLAG_PARSE_ZLIB_HEADER) - { + if (decomp_flags & TINFL_FLAG_PARSE_ZLIB_HEADER) { TINFL_SKIP_BITS(32, num_bits & 7); for (counter = 0; counter < 4; ++counter) { mz_uint s; if (num_bits) TINFL_GET_BITS(41, s, 8); else TINFL_GET_BYTE(42, s); r->m_z_adler32 = (r->m_z_adler32 << 8) | s; } } TINFL_CR_RETURN_FOREVER(34, TINFL_STATUS_DONE); TINFL_CR_FINISH -common_exit: + common_exit : r->m_num_bits = num_bits; r->m_bit_buf = bit_buf; r->m_dist = dist; r->m_counter = counter; r->m_num_extra = num_extra; r->m_dist_from_out_buf_start = dist_from_out_buf_start; *pIn_buf_size = pIn_buf_cur - pIn_buf_next; *pOut_buf_size = pOut_buf_cur - pOut_buf_next; //if ((decomp_flags & (TINFL_FLAG_PARSE_ZLIB_HEADER | TINFL_FLAG_COMPUTE_ADLER32)) && (status >= 0)) - if ((decomp_flags & TINFL_FLAG_COMPUTE_ADLER32) && (status >= 0)) - { + if ((decomp_flags & TINFL_FLAG_COMPUTE_ADLER32) && (status >= 0)) { const mz_uint8 *ptr = pOut_buf_next; size_t buf_len = *pOut_buf_size; mz_uint32 i, s1 = r->m_check_adler32 & 0xffff, s2 = r->m_check_adler32 >> 16; size_t block_len = buf_len % 5552; - while (buf_len) - { - for (i = 0; i + 7 < block_len; i += 8, ptr += 8) - { + while (buf_len) { + for (i = 0; i + 7 < block_len; i += 8, ptr += 8) { s1 += ptr[0], s2 += s1; s1 += ptr[1], s2 += s1; s1 += ptr[2], s2 += s1; s1 += ptr[3], s2 += s1; s1 += ptr[4], s2 += s1; s1 += ptr[5], s2 += s1; s1 += ptr[6], s2 += s1; s1 += ptr[7], s2 += s1; } - for ( ; i < block_len; ++i) s1 += *ptr++, s2 += s1; + for (; i < block_len; ++i) s1 += *ptr++, s2 += s1; s1 %= 65521U, s2 %= 65521U; buf_len -= block_len; block_len = 5552; } - r->m_check_adler32 = (s2 << 16) + s1; - if ((status == TINFL_STATUS_DONE) && (decomp_flags & TINFL_FLAG_PARSE_ZLIB_HEADER) && (r->m_check_adler32 != r->m_z_adler32)) - status = TINFL_STATUS_ADLER32_MISMATCH; + r->m_check_adler32 = (s2 << 16) + s1; + if ((status == TINFL_STATUS_DONE) && (decomp_flags & TINFL_FLAG_PARSE_ZLIB_HEADER) && (r->m_check_adler32 != r->m_z_adler32)) + status = TINFL_STATUS_ADLER32_MISMATCH; } return status; } @@ -1401,13 +1350,11 @@ void *tinfl_decompress_mem_to_heap(const void *pSrc_buf, size_t src_buf_len, siz tinfl_decompressor decomp; void *pBuf = NULL, *pNew_buf; size_t src_buf_ofs = 0, out_buf_capacity = 0; *pOut_len = 0; tinfl_init(&decomp); - for ( ; ; ) - { + for (; ; ) { size_t src_buf_size = src_buf_len - src_buf_ofs, dst_buf_size = out_buf_capacity - *pOut_len, new_out_buf_capacity; - tinfl_status status = tinfl_decompress(&decomp, (const mz_uint8*)pSrc_buf + src_buf_ofs, &src_buf_size, (mz_uint8*)pBuf, pBuf ? (mz_uint8*)pBuf + *pOut_len : NULL, &dst_buf_size, + tinfl_status status = tinfl_decompress(&decomp, (const mz_uint8 *)pSrc_buf + src_buf_ofs, &src_buf_size, (mz_uint8 *)pBuf, pBuf ? (mz_uint8 *)pBuf + *pOut_len : NULL, &dst_buf_size, (flags & ~TINFL_FLAG_HAS_MORE_INPUT) | TINFL_FLAG_USING_NON_WRAPPING_OUTPUT_BUF); - if ((status < 0) || (status == TINFL_STATUS_NEEDS_MORE_INPUT)) - { + if ((status < 0) || (status == TINFL_STATUS_NEEDS_MORE_INPUT)) { MZ_FREE(pBuf); *pOut_len = 0; return NULL; } src_buf_ofs += src_buf_size; @@ -1415,8 +1362,7 @@ void *tinfl_decompress_mem_to_heap(const void *pSrc_buf, size_t src_buf_len, siz if (status == TINFL_STATUS_DONE) break; new_out_buf_capacity = out_buf_capacity * 2; if (new_out_buf_capacity < 128) new_out_buf_capacity = 128; pNew_buf = MZ_REALLOC(pBuf, new_out_buf_capacity); - if (!pNew_buf) - { + if (!pNew_buf) { MZ_FREE(pBuf); *pOut_len = 0; return NULL; } pBuf = pNew_buf; out_buf_capacity = new_out_buf_capacity; @@ -1427,7 +1373,7 @@ void *tinfl_decompress_mem_to_heap(const void *pSrc_buf, size_t src_buf_len, siz size_t tinfl_decompress_mem_to_mem(void *pOut_buf, size_t out_buf_len, const void *pSrc_buf, size_t src_buf_len, int flags) { tinfl_decompressor decomp; tinfl_status status; tinfl_init(&decomp); - status = tinfl_decompress(&decomp, (const mz_uint8*)pSrc_buf, &src_buf_len, (mz_uint8*)pOut_buf, (mz_uint8*)pOut_buf, &out_buf_len, (flags & ~TINFL_FLAG_HAS_MORE_INPUT) | TINFL_FLAG_USING_NON_WRAPPING_OUTPUT_BUF); + status = tinfl_decompress(&decomp, (const mz_uint8 *)pSrc_buf, &src_buf_len, (mz_uint8 *)pOut_buf, (mz_uint8 *)pOut_buf, &out_buf_len, (flags & ~TINFL_FLAG_HAS_MORE_INPUT) | TINFL_FLAG_USING_NON_WRAPPING_OUTPUT_BUF); return (status != TINFL_STATUS_DONE) ? TINFL_DECOMPRESS_MEM_TO_MEM_FAILED : out_buf_len; } @@ -1435,20 +1381,18 @@ int tinfl_decompress_mem_to_callback(const void *pIn_buf, size_t *pIn_buf_size, { int result = 0; tinfl_decompressor decomp; - mz_uint8 *pDict = (mz_uint8*)MZ_MALLOC(TINFL_LZ_DICT_SIZE); size_t in_buf_ofs = 0, dict_ofs = 0; + mz_uint8 *pDict = (mz_uint8 *)MZ_MALLOC(TINFL_LZ_DICT_SIZE); size_t in_buf_ofs = 0, dict_ofs = 0; if (!pDict) return TINFL_STATUS_FAILED; tinfl_init(&decomp); - for ( ; ; ) - { + for (; ; ) { size_t in_buf_size = *pIn_buf_size - in_buf_ofs, dst_buf_size = TINFL_LZ_DICT_SIZE - dict_ofs; - tinfl_status status = tinfl_decompress(&decomp, (const mz_uint8*)pIn_buf + in_buf_ofs, &in_buf_size, pDict, pDict + dict_ofs, &dst_buf_size, + tinfl_status status = tinfl_decompress(&decomp, (const mz_uint8 *)pIn_buf + in_buf_ofs, &in_buf_size, pDict, pDict + dict_ofs, &dst_buf_size, (flags & ~(TINFL_FLAG_HAS_MORE_INPUT | TINFL_FLAG_USING_NON_WRAPPING_OUTPUT_BUF))); in_buf_ofs += in_buf_size; if ((dst_buf_size) && (!(*pPut_buf_func)(pDict + dict_ofs, (int)dst_buf_size, pPut_buf_user))) break; - if (status != TINFL_STATUS_HAS_MORE_OUTPUT) - { + if (status != TINFL_STATUS_HAS_MORE_OUTPUT) { result = (status == TINFL_STATUS_DONE); break; } @@ -1514,18 +1458,17 @@ static const mz_uint8 s_tdefl_large_dist_extra[128] = { // Radix sorts tdefl_sym_freq[] array by 16-bit key m_key. Returns ptr to sorted values. typedef struct { mz_uint16 m_key, m_sym_index; } tdefl_sym_freq; -static tdefl_sym_freq* tdefl_radix_sort_syms(mz_uint num_syms, tdefl_sym_freq* pSyms0, tdefl_sym_freq* pSyms1) +static tdefl_sym_freq *tdefl_radix_sort_syms(mz_uint num_syms, tdefl_sym_freq *pSyms0, tdefl_sym_freq *pSyms1) { - mz_uint32 total_passes = 2, pass_shift, pass, i, hist[256 * 2]; tdefl_sym_freq* pCur_syms = pSyms0, *pNew_syms = pSyms1; MZ_CLEAR_OBJ(hist); + mz_uint32 total_passes = 2, pass_shift, pass, i, hist[256 * 2]; tdefl_sym_freq *pCur_syms = pSyms0, *pNew_syms = pSyms1; MZ_CLEAR_OBJ(hist); for (i = 0; i < num_syms; i++) { mz_uint freq = pSyms0[i].m_key; hist[freq & 0xFF]++; hist[256 + ((freq >> 8) & 0xFF)]++; } while ((total_passes > 1) && (num_syms == hist[(total_passes - 1) * 256])) total_passes--; - for (pass_shift = 0, pass = 0; pass < total_passes; pass++, pass_shift += 8) - { - const mz_uint32* pHist = &hist[pass << 8]; + for (pass_shift = 0, pass = 0; pass < total_passes; pass++, pass_shift += 8) { + const mz_uint32 *pHist = &hist[pass << 8]; mz_uint offsets[256], cur_ofs = 0; for (i = 0; i < 256; i++) { offsets[i] = cur_ofs; cur_ofs += pHist[i]; } for (i = 0; i < num_syms; i++) pNew_syms[offsets[(pCur_syms[i].m_key >> pass_shift) & 0xFF]++] = pCur_syms[i]; - { tdefl_sym_freq* t = pCur_syms; pCur_syms = pNew_syms; pNew_syms = t; } + { tdefl_sym_freq *t = pCur_syms; pCur_syms = pNew_syms; pNew_syms = t; } } return pCur_syms; } @@ -1536,15 +1479,15 @@ static void tdefl_calculate_minimum_redundancy(tdefl_sym_freq *A, int n) int root, leaf, next, avbl, used, dpth; if (n==0) return; else if (n==1) { A[0].m_key = 1; return; } A[0].m_key += A[1].m_key; root = 0; leaf = 2; - for (next=1; next < n-1; next++) - { - if (leaf>=n || A[root].m_key=n || (root=n || A[root].m_key=n || (root=0; next--) A[next].m_key = A[A[next].m_key].m_key+1; + A[n-2].m_key = 0; for (next = n-3; next>=0; next--) A[next].m_key = A[A[next].m_key].m_key+1; avbl = 1; used = dpth = 0; root = n-2; next = n-1; - while (avbl>0) - { + while (avbl>0) { while (root>=0 && (int)A[root].m_key==dpth) { used++; root--; } while (avbl>used) { A[next--].m_key = (mz_uint16)(dpth); avbl--; } avbl = 2*used; dpth++; used = 0; @@ -1556,10 +1499,9 @@ enum { TDEFL_MAX_SUPPORTED_HUFF_CODESIZE = 32 }; static void tdefl_huffman_enforce_max_code_size(int *pNum_codes, int code_list_len, int max_code_size) { int i; mz_uint32 total = 0; if (code_list_len <= 1) return; - for (i = max_code_size + 1; i <= TDEFL_MAX_SUPPORTED_HUFF_CODESIZE; i++) pNum_codes[max_code_size] += pNum_codes[i]; + for (i = max_code_size + 1; i <= TDEFL_MAX_SUPPORTED_HUFF_CODESIZE; ++i) pNum_codes[max_code_size] += pNum_codes[i]; for (i = max_code_size; i > 0; i--) total += (((mz_uint32)pNum_codes[i]) << (max_code_size - i)); - while (total != (1UL << max_code_size)) - { + while (total != (1UL << max_code_size)) { pNum_codes[max_code_size]--; for (i = max_code_size - 1; i > 0; i--) if (pNum_codes[i]) { pNum_codes[i]--; pNum_codes[i + 1] += 2; break; } total--; @@ -1569,32 +1511,29 @@ static void tdefl_huffman_enforce_max_code_size(int *pNum_codes, int code_list_l static void tdefl_optimize_huffman_table(tdefl_compressor *d, int table_num, int table_len, int code_size_limit, int static_table) { int i, j, l, num_codes[1 + TDEFL_MAX_SUPPORTED_HUFF_CODESIZE]; mz_uint next_code[TDEFL_MAX_SUPPORTED_HUFF_CODESIZE + 1]; MZ_CLEAR_OBJ(num_codes); - if (static_table) - { + if (static_table) { for (i = 0; i < table_len; i++) num_codes[d->m_huff_code_sizes[table_num][i]]++; } - else - { + else { tdefl_sym_freq syms0[TDEFL_MAX_HUFF_SYMBOLS], syms1[TDEFL_MAX_HUFF_SYMBOLS], *pSyms; int num_used_syms = 0; const mz_uint16 *pSym_count = &d->m_huff_count[table_num][0]; - for (i = 0; i < table_len; i++) if (pSym_count[i]) { syms0[num_used_syms].m_key = (mz_uint16)pSym_count[i]; syms0[num_used_syms++].m_sym_index = (mz_uint16)i; } + for (i = 0; i < table_len; ++i) if (pSym_count[i]) { syms0[num_used_syms].m_key = (mz_uint16)pSym_count[i]; syms0[num_used_syms++].m_sym_index = (mz_uint16)i; } pSyms = tdefl_radix_sort_syms(num_used_syms, syms0, syms1); tdefl_calculate_minimum_redundancy(pSyms, num_used_syms); - for (i = 0; i < num_used_syms; i++) num_codes[pSyms[i].m_key]++; + for (i = 0; i < num_used_syms; ++i) num_codes[pSyms[i].m_key]++; tdefl_huffman_enforce_max_code_size(num_codes, num_used_syms, code_size_limit); MZ_CLEAR_OBJ(d->m_huff_code_sizes[table_num]); MZ_CLEAR_OBJ(d->m_huff_codes[table_num]); - for (i = 1, j = num_used_syms; i <= code_size_limit; i++) + for (i = 1, j = num_used_syms; i <= code_size_limit; ++i) for (l = num_codes[i]; l > 0; l--) d->m_huff_code_sizes[table_num][pSyms[--j].m_sym_index] = (mz_uint8)(i); } - next_code[1] = 0; for (j = 0, i = 2; i <= code_size_limit; i++) next_code[i] = j = ((j + num_codes[i - 1]) << 1); + next_code[1] = 0; for (j = 0, i = 2; i <= code_size_limit; ++i) next_code[i] = j = ((j + num_codes[i - 1]) << 1); - for (i = 0; i < table_len; i++) - { + for (i = 0; i < table_len; ++i) { mz_uint rev_code = 0, code, code_size; if ((code_size = d->m_huff_code_sizes[table_num][i]) == 0) continue; code = next_code[code_size]++; for (l = code_size; l > 0; l--, code >>= 1) rev_code = (rev_code << 1) | (code & 1); d->m_huff_codes[table_num][i] = (mz_uint16)rev_code; @@ -1650,30 +1589,26 @@ static void tdefl_start_dynamic_block(tdefl_compressor *d) total_code_sizes_to_pack = num_lit_codes + num_dist_codes; num_packed_code_sizes = 0; rle_z_count = 0; rle_repeat_count = 0; memset(&d->m_huff_count[2][0], 0, sizeof(d->m_huff_count[2][0]) * TDEFL_MAX_HUFF_SYMBOLS_2); - for (i = 0; i < total_code_sizes_to_pack; i++) - { + for (i = 0; i < total_code_sizes_to_pack; ++i) { mz_uint8 code_size = code_sizes_to_pack[i]; - if (!code_size) - { + if (!code_size) { TDEFL_RLE_PREV_CODE_SIZE(); if (++rle_z_count == 138) { TDEFL_RLE_ZERO_CODE_SIZE(); } } - else - { + else { TDEFL_RLE_ZERO_CODE_SIZE(); - if (code_size != prev_code_size) - { + if (code_size != prev_code_size) { TDEFL_RLE_PREV_CODE_SIZE(); d->m_huff_count[2][code_size] = (mz_uint16)(d->m_huff_count[2][code_size] + 1); packed_code_sizes[num_packed_code_sizes++] = code_size; } - else if (++rle_repeat_count == 6) - { + else if (++rle_repeat_count == 6) { TDEFL_RLE_PREV_CODE_SIZE(); } } prev_code_size = code_size; } - if (rle_repeat_count) { TDEFL_RLE_PREV_CODE_SIZE(); } else { TDEFL_RLE_ZERO_CODE_SIZE(); } + if (rle_repeat_count) { TDEFL_RLE_PREV_CODE_SIZE(); } + else { TDEFL_RLE_ZERO_CODE_SIZE(); } tdefl_optimize_huffman_table(d, 2, TDEFL_MAX_HUFF_SYMBOLS_2, 7, MZ_FALSE); @@ -1684,10 +1619,9 @@ static void tdefl_start_dynamic_block(tdefl_compressor *d) for (num_bit_lengths = 18; num_bit_lengths >= 0; num_bit_lengths--) if (d->m_huff_code_sizes[2][s_tdefl_packed_code_size_syms_swizzle[num_bit_lengths]]) break; num_bit_lengths = MZ_MAX(4, (num_bit_lengths + 1)); TDEFL_PUT_BITS(num_bit_lengths - 4, 4); - for (i = 0; (int)i < num_bit_lengths; i++) TDEFL_PUT_BITS(d->m_huff_code_sizes[2][s_tdefl_packed_code_size_syms_swizzle[i]], 3); + for (i = 0; static_cast(i) < num_bit_lengths; ++i) TDEFL_PUT_BITS(d->m_huff_code_sizes[2][s_tdefl_packed_code_size_syms_swizzle[i]], 3); - for (packed_code_sizes_index = 0; packed_code_sizes_index < num_packed_code_sizes; ) - { + for (packed_code_sizes_index = 0; packed_code_sizes_index < num_packed_code_sizes; ) { mz_uint code = packed_code_sizes[packed_code_sizes_index++]; MZ_ASSERT(code < TDEFL_MAX_HUFF_SYMBOLS_2); TDEFL_PUT_BITS(d->m_huff_codes[2][code], d->m_huff_code_sizes[2][code]); if (code >= 16) TDEFL_PUT_BITS(packed_code_sizes[packed_code_sizes_index++], "\02\03\07"[code - 16]); @@ -1700,9 +1634,9 @@ static void tdefl_start_static_block(tdefl_compressor *d) mz_uint8 *p = &d->m_huff_code_sizes[0][0]; for (i = 0; i <= 143; ++i) *p++ = 8; - for ( ; i <= 255; ++i) *p++ = 9; - for ( ; i <= 279; ++i) *p++ = 7; - for ( ; i <= 287; ++i) *p++ = 8; + for (; i <= 255; ++i) *p++ = 9; + for (; i <= 279; ++i) *p++ = 7; + for (; i <= 287; ++i) *p++ = 8; memset(d->m_huff_code_sizes[1], 5, 32); @@ -1727,13 +1661,11 @@ static mz_bool tdefl_compress_lz_codes(tdefl_compressor *d) #define TDEFL_PUT_BITS_FAST(b, l) { bit_buffer |= (((mz_uint64)(b)) << bits_in); bits_in += (l); } flags = 1; - for (pLZ_codes = d->m_lz_code_buf; pLZ_codes < pLZ_code_buf_end; flags >>= 1) - { + for (pLZ_codes = d->m_lz_code_buf; pLZ_codes < pLZ_code_buf_end; flags >>= 1) { if (flags == 1) flags = *pLZ_codes++ | 0x100; - if (flags & 1) - { + if (flags & 1) { mz_uint s0, s1, n0, n1, sym, num_extra_bits; mz_uint match_len = pLZ_codes[0], match_dist = *(const mz_uint16 *)(pLZ_codes + 1); pLZ_codes += 3; @@ -1753,21 +1685,18 @@ static mz_bool tdefl_compress_lz_codes(tdefl_compressor *d) TDEFL_PUT_BITS_FAST(d->m_huff_codes[1][sym], d->m_huff_code_sizes[1][sym]); TDEFL_PUT_BITS_FAST(match_dist & mz_bitmasks[num_extra_bits], num_extra_bits); } - else - { + else { mz_uint lit = *pLZ_codes++; MZ_ASSERT(d->m_huff_code_sizes[0][lit]); TDEFL_PUT_BITS_FAST(d->m_huff_codes[0][lit], d->m_huff_code_sizes[0][lit]); - if (((flags & 2) == 0) && (pLZ_codes < pLZ_code_buf_end)) - { + if (((flags & 2) == 0) && (pLZ_codes < pLZ_code_buf_end)) { flags >>= 1; lit = *pLZ_codes++; MZ_ASSERT(d->m_huff_code_sizes[0][lit]); TDEFL_PUT_BITS_FAST(d->m_huff_codes[0][lit], d->m_huff_code_sizes[0][lit]); - if (((flags & 2) == 0) && (pLZ_codes < pLZ_code_buf_end)) - { + if (((flags & 2) == 0) && (pLZ_codes < pLZ_code_buf_end)) { flags >>= 1; lit = *pLZ_codes++; MZ_ASSERT(d->m_huff_code_sizes[0][lit]); @@ -1779,7 +1708,7 @@ static mz_bool tdefl_compress_lz_codes(tdefl_compressor *d) if (pOutput_buf >= d->m_pOutput_buf_end) return MZ_FALSE; - *(mz_uint64*)pOutput_buf = bit_buffer; + *(mz_uint64 *)pOutput_buf = bit_buffer; pOutput_buf += (bits_in >> 3); bit_buffer >>= (bits_in & ~7); bits_in &= 7; @@ -1791,8 +1720,7 @@ static mz_bool tdefl_compress_lz_codes(tdefl_compressor *d) d->m_bits_in = 0; d->m_bit_buffer = 0; - while (bits_in) - { + while (bits_in) { mz_uint32 n = MZ_MIN(bits_in, 16); TDEFL_PUT_BITS((mz_uint)bit_buffer & mz_bitmasks[n], n); bit_buffer >>= n; @@ -1810,12 +1738,10 @@ static mz_bool tdefl_compress_lz_codes(tdefl_compressor *d) mz_uint8 *pLZ_codes; flags = 1; - for (pLZ_codes = d->m_lz_code_buf; pLZ_codes < d->m_pLZ_code_buf; flags >>= 1) - { + for (pLZ_codes = d->m_lz_code_buf; pLZ_codes < d->m_pLZ_code_buf; flags >>= 1) { if (flags == 1) flags = *pLZ_codes++ | 0x100; - if (flags & 1) - { + if (flags & 1) { mz_uint sym, num_extra_bits; mz_uint match_len = pLZ_codes[0], match_dist = (pLZ_codes[1] | (pLZ_codes[2] << 8)); pLZ_codes += 3; @@ -1823,20 +1749,17 @@ static mz_bool tdefl_compress_lz_codes(tdefl_compressor *d) TDEFL_PUT_BITS(d->m_huff_codes[0][s_tdefl_len_sym[match_len]], d->m_huff_code_sizes[0][s_tdefl_len_sym[match_len]]); TDEFL_PUT_BITS(match_len & mz_bitmasks[s_tdefl_len_extra[match_len]], s_tdefl_len_extra[match_len]); - if (match_dist < 512) - { + if (match_dist < 512) { sym = s_tdefl_small_dist_sym[match_dist]; num_extra_bits = s_tdefl_small_dist_extra[match_dist]; } - else - { + else { sym = s_tdefl_large_dist_sym[match_dist >> 8]; num_extra_bits = s_tdefl_large_dist_extra[match_dist >> 8]; } MZ_ASSERT(d->m_huff_code_sizes[1][sym]); TDEFL_PUT_BITS(d->m_huff_codes[1][sym], d->m_huff_code_sizes[1][sym]); TDEFL_PUT_BITS(match_dist & mz_bitmasks[num_extra_bits], num_extra_bits); } - else - { + else { mz_uint lit = *pLZ_codes++; MZ_ASSERT(d->m_huff_code_sizes[0][lit]); TDEFL_PUT_BITS(d->m_huff_codes[0][lit], d->m_huff_code_sizes[0][lit]); @@ -1876,8 +1799,7 @@ static int tdefl_flush_block(tdefl_compressor *d, int flush) *d->m_pLZ_flags = (mz_uint8)(*d->m_pLZ_flags >> d->m_num_flags_left); d->m_pLZ_code_buf -= (d->m_num_flags_left == 8); - if ((d->m_flags & TDEFL_WRITE_ZLIB_HEADER) && (!d->m_block_index)) - { + if ((d->m_flags & TDEFL_WRITE_ZLIB_HEADER) && (!d->m_block_index)) { TDEFL_PUT_BITS(0x78, 8); TDEFL_PUT_BITS(0x01, 8); } @@ -1889,37 +1811,30 @@ static int tdefl_flush_block(tdefl_compressor *d, int flush) comp_block_succeeded = tdefl_compress_block(d, (d->m_flags & TDEFL_FORCE_ALL_STATIC_BLOCKS) || (d->m_total_lz_bytes < 48)); // If the block gets expanded, forget the current contents of the output buffer and send a raw block instead. - if ( ((use_raw_block) || ((d->m_total_lz_bytes) && ((d->m_pOutput_buf - pSaved_output_buf + 1U) >= d->m_total_lz_bytes))) && - ((d->m_lookahead_pos - d->m_lz_code_buf_dict_pos) <= d->m_dict_size) ) - { + if (((use_raw_block) || ((d->m_total_lz_bytes) && ((d->m_pOutput_buf - pSaved_output_buf + 1U) >= d->m_total_lz_bytes))) && + ((d->m_lookahead_pos - d->m_lz_code_buf_dict_pos) <= d->m_dict_size)) { mz_uint i; d->m_pOutput_buf = pSaved_output_buf; d->m_bit_buffer = saved_bit_buf, d->m_bits_in = saved_bits_in; TDEFL_PUT_BITS(0, 2); if (d->m_bits_in) { TDEFL_PUT_BITS(0, 8 - d->m_bits_in); } - for (i = 2; i; --i, d->m_total_lz_bytes ^= 0xFFFF) - { + for (i = 2; i; --i, d->m_total_lz_bytes ^= 0xFFFF) { TDEFL_PUT_BITS(d->m_total_lz_bytes & 0xFFFF, 16); } - for (i = 0; i < d->m_total_lz_bytes; ++i) - { + for (i = 0; i < d->m_total_lz_bytes; ++i) { TDEFL_PUT_BITS(d->m_dict[(d->m_lz_code_buf_dict_pos + i) & TDEFL_LZ_DICT_SIZE_MASK], 8); } } // Check for the extremely unlikely (if not impossible) case of the compressed block not fitting into the output buffer when using dynamic codes. - else if (!comp_block_succeeded) - { + else if (!comp_block_succeeded) { d->m_pOutput_buf = pSaved_output_buf; d->m_bit_buffer = saved_bit_buf, d->m_bits_in = saved_bits_in; tdefl_compress_block(d, MZ_TRUE); } - if (flush) - { - if (flush == TDEFL_FINISH) - { + if (flush) { + if (flush == TDEFL_FINISH) { if (d->m_bits_in) { TDEFL_PUT_BITS(0, 8 - d->m_bits_in); } - if (d->m_flags & TDEFL_WRITE_ZLIB_HEADER) { mz_uint i, a = d->m_adler32; for (i = 0; i < 4; i++) { TDEFL_PUT_BITS((a >> 24) & 0xFF, 8); a <<= 8; } } + if (d->m_flags & TDEFL_WRITE_ZLIB_HEADER) { mz_uint i, a = d->m_adler32; for (i = 0; i < 4; ++i) { TDEFL_PUT_BITS((a >> 24) & 0xFF, 8); a <<= 8; } } } - else - { + else { mz_uint i, z = 0; TDEFL_PUT_BITS(0, 3); if (d->m_bits_in) { TDEFL_PUT_BITS(0, 8 - d->m_bits_in); } for (i = 2; i; --i, z ^= 0xFFFF) { TDEFL_PUT_BITS(z & 0xFFFF, 16); } } } @@ -1931,27 +1846,22 @@ static int tdefl_flush_block(tdefl_compressor *d, int flush) d->m_pLZ_code_buf = d->m_lz_code_buf + 1; d->m_pLZ_flags = d->m_lz_code_buf; d->m_num_flags_left = 8; d->m_lz_code_buf_dict_pos += d->m_total_lz_bytes; d->m_total_lz_bytes = 0; d->m_block_index++; - if ((n = (int)(d->m_pOutput_buf - pOutput_buf_start)) != 0) - { - if (d->m_pPut_buf_func) - { + if ((n = (int)(d->m_pOutput_buf - pOutput_buf_start)) != 0) { + if (d->m_pPut_buf_func) { *d->m_pIn_buf_size = d->m_pSrc - (const mz_uint8 *)d->m_pIn_buf; if (!(*d->m_pPut_buf_func)(d->m_output_buf, n, d->m_pPut_buf_user)) return (d->m_prev_return_status = TDEFL_STATUS_PUT_BUF_FAILED); } - else if (pOutput_buf_start == d->m_output_buf) - { + else if (pOutput_buf_start == d->m_output_buf) { int bytes_to_copy = (int)MZ_MIN((size_t)n, (size_t)(*d->m_pOut_buf_size - d->m_out_buf_ofs)); memcpy((mz_uint8 *)d->m_pOut_buf + d->m_out_buf_ofs, d->m_output_buf, bytes_to_copy); d->m_out_buf_ofs += bytes_to_copy; - if ((n -= bytes_to_copy) != 0) - { + if ((n -= bytes_to_copy) != 0) { d->m_output_flush_ofs = bytes_to_copy; d->m_output_flush_remaining = n; } } - else - { + else { d->m_out_buf_ofs += n; } } @@ -1965,15 +1875,13 @@ static MZ_FORCEINLINE void tdefl_find_match(tdefl_compressor *d, mz_uint lookahe { mz_uint dist, pos = lookahead_pos & TDEFL_LZ_DICT_SIZE_MASK, match_len = *pMatch_len, probe_pos = pos, next_probe_pos, probe_len; mz_uint num_probes_left = d->m_max_probes[match_len >= 32]; - const mz_uint16 *s = (const mz_uint16*)(d->m_dict + pos), *p, *q; + const mz_uint16 *s = (const mz_uint16 *)(d->m_dict + pos), *p, *q; mz_uint16 c01 = TDEFL_READ_UNALIGNED_WORD(&d->m_dict[pos + match_len - 1]), s01 = TDEFL_READ_UNALIGNED_WORD(s); MZ_ASSERT(max_match_len <= TDEFL_MAX_MATCH_LEN); if (max_match_len <= match_len) return; - for ( ; ; ) - { - for ( ; ; ) - { + for (; ; ) { + for (; ; ) { if (--num_probes_left == 0) return; - #define TDEFL_PROBE \ +#define TDEFL_PROBE \ next_probe_pos = d->m_next[probe_pos]; \ if ((!next_probe_pos) || ((dist = (mz_uint16)(lookahead_pos - next_probe_pos)) > max_dist)) return; \ probe_pos = next_probe_pos & TDEFL_LZ_DICT_SIZE_MASK; \ @@ -1983,19 +1891,17 @@ static MZ_FORCEINLINE void tdefl_find_match(tdefl_compressor *d, mz_uint lookahe if (!dist) { break; } - q = (const mz_uint16*)(d->m_dict + probe_pos); + q = (const mz_uint16 *)(d->m_dict + probe_pos); if (TDEFL_READ_UNALIGNED_WORD(q) != s01) { continue; } p = s; probe_len = 32; - do { } while ( (TDEFL_READ_UNALIGNED_WORD(++p) == TDEFL_READ_UNALIGNED_WORD(++q)) && (TDEFL_READ_UNALIGNED_WORD(++p) == TDEFL_READ_UNALIGNED_WORD(++q)) && - (TDEFL_READ_UNALIGNED_WORD(++p) == TDEFL_READ_UNALIGNED_WORD(++q)) && (TDEFL_READ_UNALIGNED_WORD(++p) == TDEFL_READ_UNALIGNED_WORD(++q)) && (--probe_len > 0) ); - if (!probe_len) - { - *pMatch_dist = dist; *pMatch_len = MZ_MIN(max_match_len, (mz_uint) TDEFL_MAX_MATCH_LEN); break; + do { } while ((TDEFL_READ_UNALIGNED_WORD(++p) == TDEFL_READ_UNALIGNED_WORD(++q)) && (TDEFL_READ_UNALIGNED_WORD(++p) == TDEFL_READ_UNALIGNED_WORD(++q)) && + (TDEFL_READ_UNALIGNED_WORD(++p) == TDEFL_READ_UNALIGNED_WORD(++q)) && (TDEFL_READ_UNALIGNED_WORD(++p) == TDEFL_READ_UNALIGNED_WORD(++q)) && (--probe_len > 0)); + if (!probe_len) { + *pMatch_dist = dist; *pMatch_len = MZ_MIN(max_match_len, (mz_uint)TDEFL_MAX_MATCH_LEN); break; } - else if ((probe_len = ((mz_uint)(p - s) * 2) + (mz_uint)(*(const mz_uint8*)p == *(const mz_uint8*)q)) > match_len) - { + else if ((probe_len = ((mz_uint)(p - s) * 2) + (mz_uint)(*(const mz_uint8 *)p == *(const mz_uint8 *)q)) > match_len) { *pMatch_dist = dist; if ((*pMatch_len = match_len = MZ_MIN(max_match_len, probe_len)) == max_match_len) break; c01 = TDEFL_READ_UNALIGNED_WORD(&d->m_dict[pos + match_len - 1]); } @@ -2009,12 +1915,10 @@ static MZ_FORCEINLINE void tdefl_find_match(tdefl_compressor *d, mz_uint lookahe const mz_uint8 *s = d->m_dict + pos, *p, *q; mz_uint8 c0 = d->m_dict[pos + match_len], c1 = d->m_dict[pos + match_len - 1]; MZ_ASSERT(max_match_len <= TDEFL_MAX_MATCH_LEN); if (max_match_len <= match_len) return; - for ( ; ; ) - { - for ( ; ; ) - { + for (; ; ) { + for (; ; ) { if (--num_probes_left == 0) return; - #define TDEFL_PROBE \ +#define TDEFL_PROBE \ next_probe_pos = d->m_next[probe_pos]; \ if ((!next_probe_pos) || ((dist = (mz_uint16)(lookahead_pos - next_probe_pos)) > max_dist)) return; \ probe_pos = next_probe_pos & TDEFL_LZ_DICT_SIZE_MASK; \ @@ -2022,8 +1926,7 @@ static MZ_FORCEINLINE void tdefl_find_match(tdefl_compressor *d, mz_uint lookahe TDEFL_PROBE; TDEFL_PROBE; TDEFL_PROBE; } if (!dist) break; p = s; q = d->m_dict + probe_pos; for (probe_len = 0; probe_len < max_match_len; probe_len++) if (*p++ != *q++) break; - if (probe_len > match_len) - { + if (probe_len > match_len) { *pMatch_dist = dist; if ((*pMatch_len = match_len = probe_len) == max_match_len) return; c0 = d->m_dict[pos + match_len]; c1 = d->m_dict[pos + match_len - 1]; } @@ -2039,16 +1942,14 @@ static mz_bool tdefl_compress_fast(tdefl_compressor *d) mz_uint8 *pLZ_code_buf = d->m_pLZ_code_buf, *pLZ_flags = d->m_pLZ_flags; mz_uint cur_pos = lookahead_pos & TDEFL_LZ_DICT_SIZE_MASK; - while ((d->m_src_buf_left) || ((d->m_flush) && (lookahead_size))) - { + while ((d->m_src_buf_left) || ((d->m_flush) && (lookahead_size))) { const mz_uint TDEFL_COMP_FAST_LOOKAHEAD_SIZE = 4096; mz_uint dst_pos = (lookahead_pos + lookahead_size) & TDEFL_LZ_DICT_SIZE_MASK; mz_uint num_bytes_to_process = (mz_uint)MZ_MIN(d->m_src_buf_left, TDEFL_COMP_FAST_LOOKAHEAD_SIZE - lookahead_size); d->m_src_buf_left -= num_bytes_to_process; lookahead_size += num_bytes_to_process; - while (num_bytes_to_process) - { + while (num_bytes_to_process) { mz_uint32 n = MZ_MIN(TDEFL_LZ_DICT_SIZE - dst_pos, num_bytes_to_process); memcpy(d->m_dict + dst_pos, d->m_pSrc, n); if (dst_pos < (TDEFL_MAX_MATCH_LEN - 1)) @@ -2061,8 +1962,7 @@ static mz_bool tdefl_compress_fast(tdefl_compressor *d) dict_size = MZ_MIN(TDEFL_LZ_DICT_SIZE - lookahead_size, dict_size); if ((!d->m_flush) && (lookahead_size < TDEFL_COMP_FAST_LOOKAHEAD_SIZE)) break; - while (lookahead_size >= 4) - { + while (lookahead_size >= 4) { mz_uint cur_match_dist, cur_match_len = 1; mz_uint8 *pCur_dict = d->m_dict + cur_pos; mz_uint first_trigram = (*(const mz_uint32 *)pCur_dict) & 0xFFFFFF; @@ -2070,26 +1970,23 @@ static mz_bool tdefl_compress_fast(tdefl_compressor *d) mz_uint probe_pos = d->m_hash[hash]; d->m_hash[hash] = (mz_uint16)lookahead_pos; - if (((cur_match_dist = (mz_uint16)(lookahead_pos - probe_pos)) <= dict_size) && ((*(const mz_uint32 *)(d->m_dict + (probe_pos &= TDEFL_LZ_DICT_SIZE_MASK)) & 0xFFFFFF) == first_trigram)) - { + if (((cur_match_dist = (mz_uint16)(lookahead_pos - probe_pos)) <= dict_size) && ((*(const mz_uint32 *)(d->m_dict + (probe_pos &= TDEFL_LZ_DICT_SIZE_MASK)) & 0xFFFFFF) == first_trigram)) { const mz_uint16 *p = (const mz_uint16 *)pCur_dict; const mz_uint16 *q = (const mz_uint16 *)(d->m_dict + probe_pos); mz_uint32 probe_len = 32; - do { } while ( (TDEFL_READ_UNALIGNED_WORD(++p) == TDEFL_READ_UNALIGNED_WORD(++q)) && (TDEFL_READ_UNALIGNED_WORD(++p) == TDEFL_READ_UNALIGNED_WORD(++q)) && - (TDEFL_READ_UNALIGNED_WORD(++p) == TDEFL_READ_UNALIGNED_WORD(++q)) && (TDEFL_READ_UNALIGNED_WORD(++p) == TDEFL_READ_UNALIGNED_WORD(++q)) && (--probe_len > 0) ); + do { } while ((TDEFL_READ_UNALIGNED_WORD(++p) == TDEFL_READ_UNALIGNED_WORD(++q)) && (TDEFL_READ_UNALIGNED_WORD(++p) == TDEFL_READ_UNALIGNED_WORD(++q)) && + (TDEFL_READ_UNALIGNED_WORD(++p) == TDEFL_READ_UNALIGNED_WORD(++q)) && (TDEFL_READ_UNALIGNED_WORD(++p) == TDEFL_READ_UNALIGNED_WORD(++q)) && (--probe_len > 0)); cur_match_len = ((mz_uint)(p - (const mz_uint16 *)pCur_dict) * 2) + (mz_uint)(*(const mz_uint8 *)p == *(const mz_uint8 *)q); if (!probe_len) cur_match_len = cur_match_dist ? TDEFL_MAX_MATCH_LEN : 0; - if ((cur_match_len < TDEFL_MIN_MATCH_LEN) || ((cur_match_len == TDEFL_MIN_MATCH_LEN) && (cur_match_dist >= 8U*1024U))) - { + if ((cur_match_len < TDEFL_MIN_MATCH_LEN) || ((cur_match_len == TDEFL_MIN_MATCH_LEN) && (cur_match_dist >= 8U*1024U))) { cur_match_len = 1; *pLZ_code_buf++ = (mz_uint8)first_trigram; *pLZ_flags = (mz_uint8)(*pLZ_flags >> 1); d->m_huff_count[0][(mz_uint8)first_trigram]++; } - else - { + else { mz_uint32 s0, s1; cur_match_len = MZ_MIN(cur_match_len, lookahead_size); @@ -2109,8 +2006,7 @@ static mz_bool tdefl_compress_fast(tdefl_compressor *d) d->m_huff_count[0][s_tdefl_len_sym[cur_match_len - TDEFL_MIN_MATCH_LEN]]++; } } - else - { + else { *pLZ_code_buf++ = (mz_uint8)first_trigram; *pLZ_flags = (mz_uint8)(*pLZ_flags >> 1); d->m_huff_count[0][(mz_uint8)first_trigram]++; @@ -2120,13 +2016,12 @@ static mz_bool tdefl_compress_fast(tdefl_compressor *d) total_lz_bytes += cur_match_len; lookahead_pos += cur_match_len; - dict_size = MZ_MIN(dict_size + cur_match_len, (mz_uint) TDEFL_LZ_DICT_SIZE); + dict_size = MZ_MIN(dict_size + cur_match_len, (mz_uint)TDEFL_LZ_DICT_SIZE); cur_pos = (cur_pos + cur_match_len) & TDEFL_LZ_DICT_SIZE_MASK; MZ_ASSERT(lookahead_size >= cur_match_len); lookahead_size -= cur_match_len; - if (pLZ_code_buf > &d->m_lz_code_buf[TDEFL_LZ_CODE_BUF_SIZE - 8]) - { + if (pLZ_code_buf > &d->m_lz_code_buf[TDEFL_LZ_CODE_BUF_SIZE - 8]) { int n; d->m_lookahead_pos = lookahead_pos; d->m_lookahead_size = lookahead_size; d->m_dict_size = dict_size; d->m_total_lz_bytes = total_lz_bytes; d->m_pLZ_code_buf = pLZ_code_buf; d->m_pLZ_flags = pLZ_flags; d->m_num_flags_left = num_flags_left; @@ -2136,8 +2031,7 @@ static mz_bool tdefl_compress_fast(tdefl_compressor *d) } } - while (lookahead_size) - { + while (lookahead_size) { mz_uint8 lit = d->m_dict[cur_pos]; total_lz_bytes++; @@ -2148,12 +2042,11 @@ static mz_bool tdefl_compress_fast(tdefl_compressor *d) d->m_huff_count[0][lit]++; lookahead_pos++; - dict_size = MZ_MIN(dict_size + 1, (mz_uint) TDEFL_LZ_DICT_SIZE); + dict_size = MZ_MIN(dict_size + 1, (mz_uint)TDEFL_LZ_DICT_SIZE); cur_pos = (cur_pos + 1) & TDEFL_LZ_DICT_SIZE_MASK; lookahead_size--; - if (pLZ_code_buf > &d->m_lz_code_buf[TDEFL_LZ_CODE_BUF_SIZE - 8]) - { + if (pLZ_code_buf > &d->m_lz_code_buf[TDEFL_LZ_CODE_BUF_SIZE - 8]) { int n; d->m_lookahead_pos = lookahead_pos; d->m_lookahead_size = lookahead_size; d->m_dict_size = dict_size; d->m_total_lz_bytes = total_lz_bytes; d->m_pLZ_code_buf = pLZ_code_buf; d->m_pLZ_flags = pLZ_flags; d->m_num_flags_left = num_flags_left; @@ -2205,38 +2098,32 @@ static mz_bool tdefl_compress_normal(tdefl_compressor *d) const mz_uint8 *pSrc = d->m_pSrc; size_t src_buf_left = d->m_src_buf_left; tdefl_flush flush = d->m_flush; - while ((src_buf_left) || ((flush) && (d->m_lookahead_size))) - { + while ((src_buf_left) || ((flush) && (d->m_lookahead_size))) { mz_uint len_to_move, cur_match_dist, cur_match_len, cur_pos; // Update dictionary and hash chains. Keeps the lookahead size equal to TDEFL_MAX_MATCH_LEN. - if ((d->m_lookahead_size + d->m_dict_size) >= (TDEFL_MIN_MATCH_LEN - 1)) - { + if ((d->m_lookahead_size + d->m_dict_size) >= (TDEFL_MIN_MATCH_LEN - 1)) { mz_uint dst_pos = (d->m_lookahead_pos + d->m_lookahead_size) & TDEFL_LZ_DICT_SIZE_MASK, ins_pos = d->m_lookahead_pos + d->m_lookahead_size - 2; mz_uint hash = (d->m_dict[ins_pos & TDEFL_LZ_DICT_SIZE_MASK] << TDEFL_LZ_HASH_SHIFT) ^ d->m_dict[(ins_pos + 1) & TDEFL_LZ_DICT_SIZE_MASK]; mz_uint num_bytes_to_process = (mz_uint)MZ_MIN(src_buf_left, TDEFL_MAX_MATCH_LEN - d->m_lookahead_size); const mz_uint8 *pSrc_end = pSrc + num_bytes_to_process; src_buf_left -= num_bytes_to_process; d->m_lookahead_size += num_bytes_to_process; - while (pSrc != pSrc_end) - { + while (pSrc != pSrc_end) { mz_uint8 c = *pSrc++; d->m_dict[dst_pos] = c; if (dst_pos < (TDEFL_MAX_MATCH_LEN - 1)) d->m_dict[TDEFL_LZ_DICT_SIZE + dst_pos] = c; hash = ((hash << TDEFL_LZ_HASH_SHIFT) ^ c) & (TDEFL_LZ_HASH_SIZE - 1); d->m_next[ins_pos & TDEFL_LZ_DICT_SIZE_MASK] = d->m_hash[hash]; d->m_hash[hash] = (mz_uint16)(ins_pos); dst_pos = (dst_pos + 1) & TDEFL_LZ_DICT_SIZE_MASK; ins_pos++; } } - else - { - while ((src_buf_left) && (d->m_lookahead_size < TDEFL_MAX_MATCH_LEN)) - { + else { + while ((src_buf_left) && (d->m_lookahead_size < TDEFL_MAX_MATCH_LEN)) { mz_uint8 c = *pSrc++; mz_uint dst_pos = (d->m_lookahead_pos + d->m_lookahead_size) & TDEFL_LZ_DICT_SIZE_MASK; src_buf_left--; d->m_dict[dst_pos] = c; if (dst_pos < (TDEFL_MAX_MATCH_LEN - 1)) d->m_dict[TDEFL_LZ_DICT_SIZE + dst_pos] = c; - if ((++d->m_lookahead_size + d->m_dict_size) >= TDEFL_MIN_MATCH_LEN) - { + if ((++d->m_lookahead_size + d->m_dict_size) >= TDEFL_MIN_MATCH_LEN) { mz_uint ins_pos = d->m_lookahead_pos + (d->m_lookahead_size - 1) - 2; mz_uint hash = ((d->m_dict[ins_pos & TDEFL_LZ_DICT_SIZE_MASK] << (TDEFL_LZ_HASH_SHIFT * 2)) ^ (d->m_dict[(ins_pos + 1) & TDEFL_LZ_DICT_SIZE_MASK] << TDEFL_LZ_HASH_SHIFT) ^ c) & (TDEFL_LZ_HASH_SIZE - 1); d->m_next[ins_pos & TDEFL_LZ_DICT_SIZE_MASK] = d->m_hash[hash]; d->m_hash[hash] = (mz_uint16)(ins_pos); @@ -2249,64 +2136,52 @@ static mz_bool tdefl_compress_normal(tdefl_compressor *d) // Simple lazy/greedy parsing state machine. len_to_move = 1; cur_match_dist = 0; cur_match_len = d->m_saved_match_len ? d->m_saved_match_len : (TDEFL_MIN_MATCH_LEN - 1); cur_pos = d->m_lookahead_pos & TDEFL_LZ_DICT_SIZE_MASK; - if (d->m_flags & (TDEFL_RLE_MATCHES | TDEFL_FORCE_ALL_RAW_BLOCKS)) - { - if ((d->m_dict_size) && (!(d->m_flags & TDEFL_FORCE_ALL_RAW_BLOCKS))) - { + if (d->m_flags & (TDEFL_RLE_MATCHES | TDEFL_FORCE_ALL_RAW_BLOCKS)) { + if ((d->m_dict_size) && (!(d->m_flags & TDEFL_FORCE_ALL_RAW_BLOCKS))) { mz_uint8 c = d->m_dict[(cur_pos - 1) & TDEFL_LZ_DICT_SIZE_MASK]; cur_match_len = 0; while (cur_match_len < d->m_lookahead_size) { if (d->m_dict[cur_pos + cur_match_len] != c) break; cur_match_len++; } if (cur_match_len < TDEFL_MIN_MATCH_LEN) cur_match_len = 0; else cur_match_dist = 1; } } - else - { + else { tdefl_find_match(d, d->m_lookahead_pos, d->m_dict_size, d->m_lookahead_size, &cur_match_dist, &cur_match_len); } - if (((cur_match_len == TDEFL_MIN_MATCH_LEN) && (cur_match_dist >= 8U*1024U)) || (cur_pos == cur_match_dist) || ((d->m_flags & TDEFL_FILTER_MATCHES) && (cur_match_len <= 5))) - { + if (((cur_match_len == TDEFL_MIN_MATCH_LEN) && (cur_match_dist >= 8U*1024U)) || (cur_pos == cur_match_dist) || ((d->m_flags & TDEFL_FILTER_MATCHES) && (cur_match_len <= 5))) { cur_match_dist = cur_match_len = 0; } - if (d->m_saved_match_len) - { - if (cur_match_len > d->m_saved_match_len) - { + if (d->m_saved_match_len) { + if (cur_match_len > d->m_saved_match_len) { tdefl_record_literal(d, (mz_uint8)d->m_saved_lit); - if (cur_match_len >= 128) - { + if (cur_match_len >= 128) { tdefl_record_match(d, cur_match_len, cur_match_dist); d->m_saved_match_len = 0; len_to_move = cur_match_len; } - else - { + else { d->m_saved_lit = d->m_dict[cur_pos]; d->m_saved_match_dist = cur_match_dist; d->m_saved_match_len = cur_match_len; } } - else - { + else { tdefl_record_match(d, d->m_saved_match_len, d->m_saved_match_dist); len_to_move = d->m_saved_match_len - 1; d->m_saved_match_len = 0; } } else if (!cur_match_dist) tdefl_record_literal(d, d->m_dict[MZ_MIN(cur_pos, sizeof(d->m_dict) - 1)]); - else if ((d->m_greedy_parsing) || (d->m_flags & TDEFL_RLE_MATCHES) || (cur_match_len >= 128)) - { + else if ((d->m_greedy_parsing) || (d->m_flags & TDEFL_RLE_MATCHES) || (cur_match_len >= 128)) { tdefl_record_match(d, cur_match_len, cur_match_dist); len_to_move = cur_match_len; } - else - { + else { d->m_saved_lit = d->m_dict[MZ_MIN(cur_pos, sizeof(d->m_dict) - 1)]; d->m_saved_match_dist = cur_match_dist; d->m_saved_match_len = cur_match_len; } // Move the lookahead forward by len_to_move bytes. d->m_lookahead_pos += len_to_move; MZ_ASSERT(d->m_lookahead_size >= len_to_move); d->m_lookahead_size -= len_to_move; - d->m_dict_size = MZ_MIN(d->m_dict_size + len_to_move, (mz_uint) TDEFL_LZ_DICT_SIZE); + d->m_dict_size = MZ_MIN(d->m_dict_size + len_to_move, (mz_uint)TDEFL_LZ_DICT_SIZE); // Check if it's time to flush the current LZ codes to the internal output buffer. - if ( (d->m_pLZ_code_buf > &d->m_lz_code_buf[TDEFL_LZ_CODE_BUF_SIZE - 8]) || - ( (d->m_total_lz_bytes > 31*1024) && (((((mz_uint)(d->m_pLZ_code_buf - d->m_lz_code_buf) * 115) >> 7) >= d->m_total_lz_bytes) || (d->m_flags & TDEFL_FORCE_ALL_RAW_BLOCKS))) ) - { + if ((d->m_pLZ_code_buf > &d->m_lz_code_buf[TDEFL_LZ_CODE_BUF_SIZE - 8]) || + ((d->m_total_lz_bytes > 31*1024) && (((((mz_uint)(d->m_pLZ_code_buf - d->m_lz_code_buf) * 115) >> 7) >= d->m_total_lz_bytes) || (d->m_flags & TDEFL_FORCE_ALL_RAW_BLOCKS)))) { int n; d->m_pSrc = pSrc; d->m_src_buf_left = src_buf_left; if ((n = tdefl_flush_block(d, 0)) != 0) @@ -2320,13 +2195,11 @@ static mz_bool tdefl_compress_normal(tdefl_compressor *d) static tdefl_status tdefl_flush_output_buffer(tdefl_compressor *d) { - if (d->m_pIn_buf_size) - { + if (d->m_pIn_buf_size) { *d->m_pIn_buf_size = d->m_pSrc - (const mz_uint8 *)d->m_pIn_buf; } - if (d->m_pOut_buf_size) - { + if (d->m_pOut_buf_size) { size_t n = MZ_MIN(*d->m_pOut_buf_size - d->m_out_buf_ofs, d->m_output_flush_remaining); memcpy((mz_uint8 *)d->m_pOut_buf + d->m_out_buf_ofs, d->m_output_buf + d->m_output_flush_ofs, n); d->m_output_flush_ofs += (mz_uint)n; @@ -2341,8 +2214,7 @@ static tdefl_status tdefl_flush_output_buffer(tdefl_compressor *d) tdefl_status tdefl_compress(tdefl_compressor *d, const void *pIn_buf, size_t *pIn_buf_size, void *pOut_buf, size_t *pOut_buf_size, tdefl_flush flush) { - if (!d) - { + if (!d) { if (pIn_buf_size) *pIn_buf_size = 0; if (pOut_buf_size) *pOut_buf_size = 0; return TDEFL_STATUS_BAD_PARAM; @@ -2354,9 +2226,8 @@ tdefl_status tdefl_compress(tdefl_compressor *d, const void *pIn_buf, size_t *pI d->m_out_buf_ofs = 0; d->m_flush = flush; - if ( ((d->m_pPut_buf_func != NULL) == ((pOut_buf != NULL) || (pOut_buf_size != NULL))) || (d->m_prev_return_status != TDEFL_STATUS_OKAY) || - (d->m_wants_to_finish && (flush != TDEFL_FINISH)) || (pIn_buf_size && *pIn_buf_size && !pIn_buf) || (pOut_buf_size && *pOut_buf_size && !pOut_buf) ) - { + if (((d->m_pPut_buf_func != NULL) == ((pOut_buf != NULL) || (pOut_buf_size != NULL))) || (d->m_prev_return_status != TDEFL_STATUS_OKAY) || + (d->m_wants_to_finish && (flush != TDEFL_FINISH)) || (pIn_buf_size && *pIn_buf_size && !pIn_buf) || (pOut_buf_size && *pOut_buf_size && !pOut_buf)) { if (pIn_buf_size) *pIn_buf_size = 0; if (pOut_buf_size) *pOut_buf_size = 0; return (d->m_prev_return_status = TDEFL_STATUS_BAD_PARAM); @@ -2369,8 +2240,7 @@ tdefl_status tdefl_compress(tdefl_compressor *d, const void *pIn_buf, size_t *pI #if MINIZ_USE_UNALIGNED_LOADS_AND_STORES && MINIZ_LITTLE_ENDIAN if (((d->m_flags & TDEFL_MAX_PROBES_MASK) == 1) && ((d->m_flags & TDEFL_GREEDY_PARSING_FLAG) != 0) && - ((d->m_flags & (TDEFL_FILTER_MATCHES | TDEFL_FORCE_ALL_RAW_BLOCKS | TDEFL_RLE_MATCHES)) == 0)) - { + ((d->m_flags & (TDEFL_FILTER_MATCHES | TDEFL_FORCE_ALL_RAW_BLOCKS | TDEFL_RLE_MATCHES)) == 0)) { if (!tdefl_compress_fast(d)) return d->m_prev_return_status; } @@ -2384,8 +2254,7 @@ tdefl_status tdefl_compress(tdefl_compressor *d, const void *pIn_buf, size_t *pI if ((d->m_flags & (TDEFL_WRITE_ZLIB_HEADER | TDEFL_COMPUTE_ADLER32)) && (pIn_buf)) d->m_adler32 = (mz_uint32)mz_adler32(d->m_adler32, (const mz_uint8 *)pIn_buf, d->m_pSrc - (const mz_uint8 *)pIn_buf); - if ((flush) && (!d->m_lookahead_size) && (!d->m_src_buf_left) && (!d->m_output_flush_remaining)) - { + if ((flush) && (!d->m_lookahead_size) && (!d->m_src_buf_left) && (!d->m_output_flush_remaining)) { if (tdefl_flush_block(d, flush) < 0) return d->m_prev_return_status; d->m_finished = (flush == TDEFL_FINISH); @@ -2432,7 +2301,7 @@ mz_uint32 tdefl_get_adler32(tdefl_compressor *d) mz_bool tdefl_compress_mem_to_output(const void *pBuf, size_t buf_len, tdefl_put_buf_func_ptr pPut_buf_func, void *pPut_buf_user, int flags) { tdefl_compressor *pComp; mz_bool succeeded; if (((buf_len) && (!pBuf)) || (!pPut_buf_func)) return MZ_FALSE; - pComp = (tdefl_compressor*)MZ_MALLOC(sizeof(tdefl_compressor)); if (!pComp) return MZ_FALSE; + pComp = (tdefl_compressor *)MZ_MALLOC(sizeof(tdefl_compressor)); if (!pComp) return MZ_FALSE; succeeded = (tdefl_init(pComp, pPut_buf_func, pPut_buf_user, flags) == TDEFL_STATUS_OKAY); succeeded = succeeded && (tdefl_compress_buffer(pComp, pBuf, buf_len, TDEFL_FINISH) == TDEFL_STATUS_DONE); MZ_FREE(pComp); return succeeded; @@ -2449,14 +2318,13 @@ static mz_bool tdefl_output_buffer_putter(const void *pBuf, int len, void *pUser { tdefl_output_buffer *p = (tdefl_output_buffer *)pUser; size_t new_size = p->m_size + len; - if (new_size > p->m_capacity) - { + if (new_size > p->m_capacity) { size_t new_capacity = p->m_capacity; mz_uint8 *pNew_buf; if (!p->m_expandable) return MZ_FALSE; do { new_capacity = MZ_MAX(128U, new_capacity << 1U); } while (new_size > new_capacity); - pNew_buf = (mz_uint8*)MZ_REALLOC(p->m_pBuf, new_capacity); if (!pNew_buf) return MZ_FALSE; + pNew_buf = (mz_uint8 *)MZ_REALLOC(p->m_pBuf, new_capacity); if (!pNew_buf) return MZ_FALSE; p->m_pBuf = pNew_buf; p->m_capacity = new_capacity; } - memcpy((mz_uint8*)p->m_pBuf + p->m_size, pBuf, len); p->m_size = new_size; + memcpy((mz_uint8 *)p->m_pBuf + p->m_size, pBuf, len); p->m_size = new_size; return MZ_TRUE; } @@ -2473,7 +2341,7 @@ size_t tdefl_compress_mem_to_mem(void *pOut_buf, size_t out_buf_len, const void { tdefl_output_buffer out_buf; MZ_CLEAR_OBJ(out_buf); if (!pOut_buf) return 0; - out_buf.m_pBuf = (mz_uint8*)pOut_buf; out_buf.m_capacity = out_buf_len; + out_buf.m_pBuf = (mz_uint8 *)pOut_buf; out_buf.m_capacity = out_buf_len; if (!tdefl_compress_mem_to_output(pSrc_buf, src_buf_len, tdefl_output_buffer_putter, &out_buf, flags)) return 0; return out_buf.m_size; } @@ -2511,26 +2379,26 @@ void *tdefl_write_image_to_png_file_in_memory_ex(const void *pImage, int w, int static const mz_uint s_tdefl_png_num_probes[11] = { 0, 1, 6, 32, 16, 32, 128, 256, 512, 768, 1500 }; tdefl_compressor *pComp = (tdefl_compressor *)MZ_MALLOC(sizeof(tdefl_compressor)); tdefl_output_buffer out_buf; int i, bpl = w * num_chans, y, z; mz_uint32 c; *pLen_out = 0; if (!pComp) return NULL; - MZ_CLEAR_OBJ(out_buf); out_buf.m_expandable = MZ_TRUE; out_buf.m_capacity = 57+MZ_MAX(64, (1+bpl)*h); if (NULL == (out_buf.m_pBuf = (mz_uint8*)MZ_MALLOC(out_buf.m_capacity))) { MZ_FREE(pComp); return NULL; } + MZ_CLEAR_OBJ(out_buf); out_buf.m_expandable = MZ_TRUE; out_buf.m_capacity = 57+MZ_MAX(64, (1+bpl)*h); if (NULL == (out_buf.m_pBuf = (mz_uint8 *)MZ_MALLOC(out_buf.m_capacity))) { MZ_FREE(pComp); return NULL; } // write dummy header for (z = 41; z; --z) tdefl_output_buffer_putter(&z, 1, &out_buf); // compress image data tdefl_init(pComp, tdefl_output_buffer_putter, &out_buf, s_tdefl_png_num_probes[MZ_MIN(10, level)] | TDEFL_WRITE_ZLIB_HEADER | (level <= 3 ? TDEFL_GREEDY_PARSING_FLAG : 0)); - for (y = 0; y < h; ++y) { tdefl_compress_buffer(pComp, &z, 1, TDEFL_NO_FLUSH); tdefl_compress_buffer(pComp, (mz_uint8*)pImage + (flip ? (h - 1 - y) : y) * bpl, bpl, TDEFL_NO_FLUSH); } + for (y = 0; y < h; ++y) { tdefl_compress_buffer(pComp, &z, 1, TDEFL_NO_FLUSH); tdefl_compress_buffer(pComp, (mz_uint8 *)pImage + (flip ? (h - 1 - y) : y) * bpl, bpl, TDEFL_NO_FLUSH); } if (tdefl_compress_buffer(pComp, NULL, 0, TDEFL_FINISH) != TDEFL_STATUS_DONE) { MZ_FREE(pComp); MZ_FREE(out_buf.m_pBuf); return NULL; } // write real header *pLen_out = out_buf.m_size-41; { - static const mz_uint8 chans[] = {0x00, 0x00, 0x04, 0x02, 0x06}; - mz_uint8 pnghdr[41]={0x89,0x50,0x4e,0x47,0x0d,0x0a,0x1a,0x0a,0x00,0x00,0x00,0x0d,0x49,0x48,0x44,0x52, + static const mz_uint8 chans[] = { 0x00, 0x00, 0x04, 0x02, 0x06 }; + mz_uint8 pnghdr[41] = { 0x89,0x50,0x4e,0x47,0x0d,0x0a,0x1a,0x0a,0x00,0x00,0x00,0x0d,0x49,0x48,0x44,0x52, 0,0,(mz_uint8)(w>>8),(mz_uint8)w,0,0,(mz_uint8)(h>>8),(mz_uint8)h,8,chans[num_chans],0,0,0,0,0,0,0, - (mz_uint8)(*pLen_out>>24),(mz_uint8)(*pLen_out>>16),(mz_uint8)(*pLen_out>>8),(mz_uint8)*pLen_out,0x49,0x44,0x41,0x54}; - c=(mz_uint32)mz_crc32(MZ_CRC32_INIT,pnghdr+12,17); for (i=0; i<4; ++i, c<<=8) ((mz_uint8*)(pnghdr+29))[i]=(mz_uint8)(c>>24); + (mz_uint8)(*pLen_out>>24),(mz_uint8)(*pLen_out>>16),(mz_uint8)(*pLen_out>>8),(mz_uint8)*pLen_out,0x49,0x44,0x41,0x54 }; + c = (mz_uint32)mz_crc32(MZ_CRC32_INIT, pnghdr+12, 17); for (i = 0; i<4; ++i, c <<= 8) ((mz_uint8 *)(pnghdr+29))[i] = (mz_uint8)(c>>24); memcpy(out_buf.m_pBuf, pnghdr, 41); } // write footer (IDAT CRC-32, followed by IEND chunk) if (!tdefl_output_buffer_putter("\0\0\0\0\0\0\0\0\x49\x45\x4e\x44\xae\x42\x60\x82", 16, &out_buf)) { *pLen_out = 0; MZ_FREE(pComp); MZ_FREE(out_buf.m_pBuf); return NULL; } - c = (mz_uint32)mz_crc32(MZ_CRC32_INIT,out_buf.m_pBuf+41-4, *pLen_out+4); for (i=0; i<4; ++i, c<<=8) (out_buf.m_pBuf+out_buf.m_size-16)[i] = (mz_uint8)(c >> 24); + c = (mz_uint32)mz_crc32(MZ_CRC32_INIT, out_buf.m_pBuf+41-4, *pLen_out+4); for (i = 0; i<4; ++i, c <<= 8) (out_buf.m_pBuf+out_buf.m_size-16)[i] = (mz_uint8)(c >> 24); // compute final size of file, grab compressed data buffer and return *pLen_out += 57; MZ_FREE(pComp); return out_buf.m_pBuf; } @@ -2547,4 +2415,3 @@ void *tdefl_write_image_to_png_file_in_memory(const void *pImage, int w, int h, } // namespace buminiz #endif // MINIZ_HEADER_FILE_ONLY - diff --git a/modules/core/src/tools/file/vpIoTools.cpp b/modules/core/src/tools/file/vpIoTools.cpp index e00bd664d6..28a256d026 100644 --- a/modules/core/src/tools/file/vpIoTools.cpp +++ b/modules/core/src/tools/file/vpIoTools.cpp @@ -1432,11 +1432,11 @@ std::string vpIoTools::path(const std::string &pathname) std::string path(pathname); #if defined(_WIN32) - for (unsigned int i = 0; i < path.length(); i++) + for (unsigned int i = 0; i < path.length(); ++i) if (path[i] == '/') path[i] = '\\'; #elif defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)) - for (unsigned int i = 0; i < path.length(); i++) + for (unsigned int i = 0; i < path.length(); ++i) if (path[i] == '\\') path[i] = '/'; #if TARGET_OS_IOS == 0 // The following code is not working on iOS and android since @@ -2073,9 +2073,9 @@ std::string vpIoTools::toLowerCase(const std::string &input) { std::string out; #if VISP_CXX_STANDARD > VISP_CXX_STANDARD_98 - for (std::string::const_iterator it = input.cbegin(); it != input.cend(); it++) { + for (std::string::const_iterator it = input.cbegin(); it != input.cend(); ++it) { #else - for (std::string::const_iterator it = input.begin(); it != input.end(); it++) { + for (std::string::const_iterator it = input.begin(); it != input.end(); ++it) { #endif out += std::tolower(*it); } @@ -2094,9 +2094,9 @@ std::string vpIoTools::toUpperCase(const std::string &input) { std::string out; #if VISP_CXX_STANDARD > VISP_CXX_STANDARD_98 - for (std::string::const_iterator it = input.cbegin(); it != input.cend(); it++) { + for (std::string::const_iterator it = input.cbegin(); it != input.cend(); ++it) { #else - for (std::string::const_iterator it = input.begin(); it != input.end(); it++) { + for (std::string::const_iterator it = input.begin(); it != input.end(); ++it) { #endif out += std::toupper(*it); } @@ -2353,7 +2353,7 @@ int main() std::vector subChain = vpIoTools::splitChain(chain, sep); std::cout << "Found the following subchains: " << std::endl; - for (size_t i=0; i < subChain.size(); i++) + for (size_t i=0; i < subChain.size(); ++i) std::cout << subChain[i] << std::endl; } @@ -2363,7 +2363,7 @@ int main() std::vector subChain = vpIoTools::splitChain(chain, sep); std::cout << "Found the following subchains: " << std::endl; - for (size_t i=0; i < subChain.size(); i++) + for (size_t i=0; i < subChain.size(); ++i) std::cout << subChain[i] << std::endl; } } @@ -2428,7 +2428,7 @@ std::vector vpIoTools::getDirFiles(const std::string &pathname) if (filesCount == -1) { throw(vpIoException(vpException::fatalError, "Cannot read files of directory %s", dirName.c_str())); } - for (int i = 0; i < filesCount; i++) { + for (int i = 0; i < filesCount; ++i) { std::string fileName = list[i]->d_name; if (fileName != "." && fileName != "..") { files.push_back(fileName); diff --git a/modules/core/src/tools/geometry/vpPlane.cpp b/modules/core/src/tools/geometry/vpPlane.cpp index 482c0647d8..aaf5ea5979 100644 --- a/modules/core/src/tools/geometry/vpPlane.cpp +++ b/modules/core/src/tools/geometry/vpPlane.cpp @@ -110,10 +110,12 @@ vpPlane::vpPlane(const vpPoint &P, const vpColVector &n, vpPlaneFrame frame) : A B = n[1]; C = n[2]; - if (frame == vpPlane::camera_frame) - D = -(A * P.get_X() + B * P.get_Y() + C * P.get_Z()); - else - D = -(A * P.get_oX() + B * P.get_oY() + C * P.get_oZ()); + if (frame == vpPlane::camera_frame) { + D = -((A * P.get_X()) + (B * P.get_Y()) + (C * P.get_Z())); + } + else { + D = -((A * P.get_oX()) + (B * P.get_oY()) + (C * P.get_oZ())); + } } /*! @@ -147,7 +149,7 @@ void vpPlane::init(const vpColVector &P, const vpColVector &n) B = n[1]; C = n[2]; - D = -(A * P[0] + B * P[1] + C * P[2]); + D = -((A * P[0]) + (B * P[1]) + (C * P[2])); } /*! @@ -195,12 +197,14 @@ void vpPlane::init(const vpPoint &P, const vpPoint &Q, const vpPoint &R, vpPlane A = n[0]; B = n[1]; C = n[2]; - if (frame == vpPlane::camera_frame) - D = -(A * P.get_X() + B * P.get_Y() + C * P.get_Z()); - else - D = -(A * P.get_oX() + B * P.get_oY() + C * P.get_oZ()); + if (frame == vpPlane::camera_frame) { + D = -((A * P.get_X()) + (B * P.get_Y()) + (C * P.get_Z())); + } + else { + D = -((A * P.get_oX()) + (B * P.get_oY()) + (C * P.get_oZ())); + } - double norm = sqrt(A * A + B * B + C * C); + double norm = sqrt((A * A) + (B * B) + (C * C)); A /= norm; B /= norm; C /= norm; @@ -231,7 +235,7 @@ vpPlane::vpPlane(const vpPoint &P, const vpPoint &Q, const vpPoint &R, vpPlaneFr */ double vpPlane::computeZ(double x, double y) const { - return -getD() / (getA() * x + getB() * y + getC()); + return -getD() / ((getA() * x) + (getB() * y) + getC()); } /*! @@ -285,11 +289,11 @@ void vpPlane::projectionPointOnPlan(const vpPoint &P, vpPoint &Pproj) const y0 = P.get_Y() / P.get_W(); z0 = P.get_Z() / P.get_W(); - rho = -(A * x0 + B * y0 + C * z0 + D) / (A * A + B * B + C * C); + rho = -((A * x0) + (B * y0) + (C * z0) + D) / ((A * A) + (B * B) + (C * C)); - Pproj.set_X(x0 + A * rho); - Pproj.set_Y(y0 + B * rho); - Pproj.set_Z(z0 + C * rho); + Pproj.set_X(x0 + (A * rho)); + Pproj.set_Y(y0 + (B * rho)); + Pproj.set_Z(z0 + (C * rho)); Pproj.set_W(1); } @@ -298,33 +302,37 @@ double vpPlane::rayIntersection(const vpPoint &M0, const vpPoint &M1, vpColVecto double k, scal; - // if(M0.get_X()!=0 || M0.get_Y()!=0 || M0.get_Z()!=0) - if (std::fabs(M0.get_X()) > std::numeric_limits::epsilon() || - std::fabs(M0.get_Y()) > std::numeric_limits::epsilon() || - std::fabs(M0.get_Z()) > std::numeric_limits::epsilon()) { + // --comment: if(M0.get_X()!=0 || M0.get_Y()!=0 || M0.get_Z()!=0) + if ((std::fabs(M0.get_X()) > std::numeric_limits::epsilon()) || + (std::fabs(M0.get_Y()) > std::numeric_limits::epsilon()) || + (std::fabs(M0.get_Z()) > std::numeric_limits::epsilon())) { double R[3]; R[0] = M1.get_X() - M0.get_X(); R[1] = M1.get_Y() - M0.get_Y(); R[2] = M1.get_Z() - M0.get_Z(); - scal = getA() * R[0] + getB() * R[1] + getC() * R[2]; - // if (scal != 0) - if (std::fabs(scal) > std::numeric_limits::epsilon()) - k = -(getA() * M0.get_X() + getB() * M0.get_Y() + getC() * M0.get_Z() + getD()) / scal; - else + scal = (getA() * R[0]) + (getB() * R[1]) + (getC() * R[2]); + // --comment: if (scal != 0) + if (std::fabs(scal) > std::numeric_limits::epsilon()) { + k = -((getA() * M0.get_X()) + (getB() * M0.get_Y()) + (getC() * M0.get_Z()) + getD()) / scal; + } + else { k = 0; + } - H[0] = M0.get_X() + k * R[0]; - H[1] = M0.get_Y() + k * R[1]; - H[2] = M0.get_Z() + k * R[2]; + H[0] = M0.get_X() + (k * R[0]); + H[1] = M0.get_Y() + (k * R[1]); + H[2] = M0.get_Z() + (k * R[2]); } else { - scal = getA() * M1.get_X() + getB() * M1.get_Y() + getC() * M1.get_Z(); - // if (scal != 0) - if (std::fabs(scal) > std::numeric_limits::epsilon()) + scal = (getA() * M1.get_X()) + (getB() * M1.get_Y()) + (getC() * M1.get_Z()); + // --comment: if (scal != 0) + if (std::fabs(scal) > std::numeric_limits::epsilon()) { k = -getD() / scal; - else + } + else { k = 0; + } H[0] = k * M1.get_X(); H[1] = k * M1.get_Y(); H[2] = k * M1.get_Z(); @@ -338,12 +346,14 @@ double vpPlane::getIntersection(const vpColVector &M1, vpColVector &H) const double k, scal; - scal = A * M1[0] + B * M1[1] + C * M1[2]; - // if (scal != 0) - if (std::fabs(scal) > std::numeric_limits::epsilon()) + scal = (A * M1[0]) + (B * M1[1]) + (C * M1[2]); + // --comment: if (scal != 0) + if (std::fabs(scal) > std::numeric_limits::epsilon()) { k = -getD() / scal; - else + } + else { k = 0; + } H[0] = k * M1[0]; H[1] = k * M1[1]; H[2] = k * M1[2]; @@ -363,10 +373,10 @@ void vpPlane::changeFrame(const vpHomogeneousMatrix &cMo) { // Save current plane parameters double Ao = A, Bo = B, Co = C, Do = D; - A = cMo[0][0] * Ao + cMo[0][1] * Bo + cMo[0][2] * Co; - B = cMo[1][0] * Ao + cMo[1][1] * Bo + cMo[1][2] * Co; - C = cMo[2][0] * Ao + cMo[2][1] * Bo + cMo[2][2] * Co; - D = Do - (cMo[0][3] * A + cMo[1][3] * B + cMo[2][3] * C); + A = (cMo[0][0] * Ao) + (cMo[0][1] * Bo) + (cMo[0][2] * Co); + B = (cMo[1][0] * Ao) + (cMo[1][1] * Bo) + (cMo[1][2] * Co); + C = (cMo[2][0] * Ao) + (cMo[2][1] * Bo) + (cMo[2][2] * Co); + D = Do - ((cMo[0][3] * A) + (cMo[1][3] * B) + (cMo[2][3] * C)); } /*! diff --git a/modules/core/src/tools/geometry/vpPolygon.cpp b/modules/core/src/tools/geometry/vpPolygon.cpp index 90c3e61198..03a81071ab 100644 --- a/modules/core/src/tools/geometry/vpPolygon.cpp +++ b/modules/core/src/tools/geometry/vpPolygon.cpp @@ -240,7 +240,8 @@ void vpPolygon::buildFrom(const std::vector &corners, const vpCameraPar const bool create_convex_hull) { std::vector ipCorners(corners.size()); - for (unsigned int i = 0; i < corners.size(); ++i) { + unsigned int corners_size = corners.size(); + for (unsigned int i = 0; i < corners_size; ++i) { vpMeterPixelConversion::convertPoint(cam, corners[i].get_x(), corners[i].get_y(), ipCorners[i]); } buildFrom(ipCorners, create_convex_hull); @@ -265,7 +266,7 @@ void vpPolygon::initClick(const vpImage &I, unsigned int size, co while (button == vpMouseButton::button1) { bool ret = vpDisplay::getClick(I, ip, button, true); - if (ret && button == vpMouseButton::button1) { + if (ret && (button == vpMouseButton::button1)) { vpDisplay::displayCross(I, ip, size, color, thickness); cornersClick.push_back(ip); vpDisplay::flush(I); @@ -293,7 +294,7 @@ void vpPolygon::initClick(const vpImage &I, unsigned int size, const vpC while (button == vpMouseButton::button1) { bool ret = vpDisplay::getClick(I, ip, button, true); - if (ret && button == vpMouseButton::button1) { + if (ret && (button == vpMouseButton::button1)) { vpDisplay::displayCross(I, ip, size, color, thickness); cornersClick.push_back(ip); vpDisplay::flush(I); @@ -363,19 +364,19 @@ bool vpPolygon::testIntersectionSegments(const vpImagePoint &ip1, const vpImageP double di2 = ip4.get_i() - ip3.get_i(); double dj2 = ip4.get_j() - ip3.get_j(); - double denominator = di1 * dj2 - dj1 * di2; + double denominator = (di1 * dj2) - (dj1 * di2); if (fabs(denominator) < std::numeric_limits::epsilon()) { throw vpException(vpException::divideByZeroError, "Denominator is null, lines are parallels"); } - double alpha = -((ip1.get_i() - ip3.get_i()) * dj2 + di2 * (ip3.get_j() - ip1.get_j())) / denominator; - if (alpha < 0 || alpha >= 1) { + double alpha = -(((ip1.get_i() - ip3.get_i()) * dj2) + (di2 * (ip3.get_j() - ip1.get_j()))) / denominator; + if ((alpha < 0) || (alpha >= 1)) { return false; } double beta = -(di1 * (ip3.get_j() - ip1.get_j()) + dj1 * (ip1.get_i() - ip3.get_i())) / denominator; - if (beta < 0 || beta >= 1) { + if ((beta < 0) || (beta >= 1)) { return false; } @@ -403,11 +404,12 @@ bool vpPolygon::isInside(const vpImagePoint &ip, const PointInPolygonMethod &met // we add random since it appears that sometimes infPoint may cause a degenerated case (so relaunch and // hope that result will be different). vpUniRand generator; - infPoint.set_i(infPoint.get_i() + 1000 * generator()); - infPoint.set_j(infPoint.get_j() + 1000 * generator()); + infPoint.set_i(infPoint.get_i() + (1000 * generator())); + infPoint.set_j(infPoint.get_j() + (1000 * generator())); bool oddNbIntersections = false; - for (unsigned int i = 0; i < _corners.size(); ++i) { + unsigned int _corners_size = _corners.size(); + for (unsigned int i = 0; i < _corners_size; ++i) { vpImagePoint ip1 = _corners[i]; vpImagePoint ip2 = _corners[(i + 1) % _corners.size()]; bool intersection = false; @@ -430,15 +432,17 @@ bool vpPolygon::isInside(const vpImagePoint &ip, const PointInPolygonMethod &met } test = oddNbIntersections; - } break; + } + break; - // Reference: http://alienryderflex.com/polygon/ + // Reference: http://alienryderflex.com/polygon/ case PnPolyRayCasting: default: { bool oddNodes = false; - for (size_t i = 0, j = _corners.size() - 1; i < _corners.size(); i++) { - if ((_corners[i].get_v() < ip.get_v() && _corners[j].get_v() >= ip.get_v()) || - (_corners[j].get_v() < ip.get_v() && _corners[i].get_v() >= ip.get_v())) { + size_t _corners_size = _corners.size(); + for (size_t i = 0, j = _corners_size - 1; i < _corners.size(); ++i) { + if (((_corners[i].get_v() < ip.get_v()) && (_corners[j].get_v() >= ip.get_v())) || + ((_corners[j].get_v() < ip.get_v()) && (_corners[i].get_v() >= ip.get_v()))) { oddNodes ^= (ip.get_v() * m_PnPolyMultiples[i] + m_PnPolyConstants[i] < ip.get_u()); } @@ -446,7 +450,8 @@ bool vpPolygon::isInside(const vpImagePoint &ip, const PointInPolygonMethod &met } test = oddNodes; - } break; + } + break; } return test; @@ -461,7 +466,8 @@ void vpPolygon::precalcValuesPnPoly() m_PnPolyConstants.resize(_corners.size()); m_PnPolyMultiples.resize(_corners.size()); - for (size_t i = 0, j = _corners.size() - 1; i < _corners.size(); i++) { + size_t _corners_size = _corners.size(); + for (size_t i = 0, j = _corners.size() - 1; i < _corners_size; ++i) { if (vpMath::equal(_corners[j].get_v(), _corners[i].get_v(), std::numeric_limits::epsilon())) { m_PnPolyConstants[i] = _corners[i].get_u(); m_PnPolyMultiples[i] = 0.0; @@ -495,9 +501,10 @@ void vpPolygon::updateArea() } _area = 0; - for (unsigned int i = 0; i < _corners.size(); ++i) { + unsigned int _corners_size = _corners.size(); + for (unsigned int i = 0; i < _corners_size; ++i) { unsigned int i_p_1 = (i + 1) % _corners.size(); - _area += _corners[i].get_j() * _corners[i_p_1].get_i() - _corners[i_p_1].get_j() * _corners[i].get_i(); + _area += (_corners[i].get_j() * _corners[i_p_1].get_i()) - (_corners[i_p_1].get_j() * _corners[i].get_i()); } _area /= 2; @@ -535,13 +542,14 @@ void vpPolygon::updateCenter() (_corners[i + 1].get_i() * _corners[i].get_j() - _corners[i + 1].get_j() * _corners[i].get_i()); } #else - for (unsigned int i = 0; i < _corners.size(); ++i) { + unsigned int _corners_size = _corners.size(); + for (unsigned int i = 0; i < _corners_size; ++i) { unsigned int i_p_1 = (i + 1) % _corners.size(); i_tmp += (_corners[i].get_i() + _corners[i_p_1].get_i()) * - (_corners[i_p_1].get_i() * _corners[i].get_j() - _corners[i_p_1].get_j() * _corners[i].get_i()); + ((_corners[i_p_1].get_i() * _corners[i].get_j()) - (_corners[i_p_1].get_j() * _corners[i].get_i())); j_tmp += (_corners[i].get_j() + _corners[i_p_1].get_j()) * - (_corners[i_p_1].get_i() * _corners[i].get_j() - _corners[i_p_1].get_j() * _corners[i].get_i()); + ((_corners[i_p_1].get_i() * _corners[i].get_j()) - (_corners[i_p_1].get_j() * _corners[i].get_i())); } #endif @@ -571,7 +579,8 @@ void vpPolygon::updateBoundingBox() std::set setI; std::set setJ; - for (unsigned int i = 0; i < _corners.size(); ++i) { + unsigned int _corners_size = _corners.size(); + for (unsigned int i = 0; i < _corners_size; ++i) { setI.insert(_corners[i].get_i()); setJ.insert(_corners[i].get_j()); } @@ -599,7 +608,7 @@ void vpPolygon::updateBoundingBox() */ void vpPolygon::display(const vpImage &I, const vpColor &color, unsigned int thickness) const { - const unsigned int N = (unsigned int)_corners.size(); + const unsigned int N = static_cast(_corners.size()); for (unsigned int i = 0; i < N; ++i) { vpDisplay::displayLine(I, _corners[i], _corners[(i + 1) % N], color, thickness); } @@ -628,5 +637,5 @@ bool vpPolygon::isInside(const std::vector &roi, const double &i, */ unsigned int vpPolygon::getSize() const { - return ((unsigned int)_corners.size()); + return (static_cast(_corners.size())); } diff --git a/modules/core/src/tools/geometry/vpRect.cpp b/modules/core/src/tools/geometry/vpRect.cpp index be24cc6406..ef2a9f5693 100644 --- a/modules/core/src/tools/geometry/vpRect.cpp +++ b/modules/core/src/tools/geometry/vpRect.cpp @@ -120,8 +120,8 @@ vpRect::vpRect(const std::vector &ip) : left(0), top(0), width(0), */ bool vpRect::isInside(const vpImagePoint &ip) const { - return (ip.get_i() <= this->getBottom() && ip.get_i() >= this->getTop() && ip.get_j() <= this->getRight() && - ip.get_j() >= this->getLeft()); + return ((ip.get_i() <= this->getBottom()) && (ip.get_i() >= this->getTop()) && (ip.get_j() <= this->getRight()) && + (ip.get_j() >= this->getLeft())); } /*! @@ -163,24 +163,30 @@ void vpRect::set(const vpImagePoint &topLeft, double w, double h) */ void vpRect::set(const std::vector &ip) { - if (ip.size() < 1) + if (ip.size() < 1) { throw(vpException(vpException::dimensionError, "At least 1 point is requested to build a rectangle")); + } double minu, maxu; double minv, maxv; minu = maxu = ip[0].get_u(); minv = maxv = ip[0].get_v(); - for (size_t i = 1; i < ip.size(); i++) { + size_t ip_size = ip.size(); + for (size_t i = 1; i < ip_size; ++i) { double u = ip[i].get_u(); double v = ip[i].get_v(); - if (u < minu) + if (u < minu) { minu = u; - else if (u > maxu) + } + else if (u > maxu) { maxu = u; - if (v < minv) + } + if (v < minv) { minv = v; - else if (v > maxv) + } + else if (v > maxv) { maxv = v; + } } setLeft(minu); @@ -216,12 +222,12 @@ void vpRect::set(const vpRect &r) { *this = r; } */ bool vpRect::operator==(const vpRect &r) const { - // return (top == r.top && left == r.left && width == r.width && height == - // r.height); - return (std::fabs(top - r.top) <= std::fabs(top) * std::numeric_limits::epsilon() && - std::fabs(left - r.left) <= std::fabs(left) * std::numeric_limits::epsilon() && - std::fabs(width - r.width) <= std::fabs(width) * std::numeric_limits::epsilon() && - std::fabs(height - r.height) <= std::fabs(height) * std::numeric_limits::epsilon()); + // --comment: return (top == r.top && left == r.left && width == r.width && height == + // --comment: r.height); + return ((std::fabs(top - r.top) <= (std::fabs(top) * std::numeric_limits::epsilon())) && + (std::fabs(left - r.left) <= (std::fabs(left) * std::numeric_limits::epsilon())) && + (std::fabs(width - r.width) <= (std::fabs(width) * std::numeric_limits::epsilon())) && + (std::fabs(height - r.height) <= (std::fabs(height) * std::numeric_limits::epsilon()))); } /*! @@ -230,16 +236,16 @@ bool vpRect::operator==(const vpRect &r) const */ bool vpRect::operator!=(const vpRect &r) const { - // return (top != r.top || left != r.left || width != r.width || height != - // r.height); - // return (std::fabs(top-r.top) > - // std::fabs(top)*std::numeric_limits::epsilon() - // || std::fabs(left-r.left) > - // std::fabs(left)*std::numeric_limits::epsilon() - // || std::fabs(width-r.width) > - // std::fabs(width)*std::numeric_limits::epsilon() - // || std::fabs(height-r.height) > - // std::fabs(height)*std::numeric_limits::epsilon()); + // --comment: return (top != r.top || left != r.left || width != r.width || height != + // --comment: r.height); + // --comment: return (std::fabs(top-r.top) > + // --comment: std::fabs(top)*std::numeric_limits::epsilon() + // --comment: || std::fabs(left-r.left) > + // --comment: std::fabs(left)*std::numeric_limits::epsilon() + // --comment: || std::fabs(width-r.width) > + // --comment: std::fabs(width)*std::numeric_limits::epsilon() + // --comment: || std::fabs(height-r.height) > + // --comment: std::fabs(height)*std::numeric_limits::epsilon()); return !(*this == r); } @@ -253,8 +259,8 @@ bool vpRect::operator!=(const vpRect &r) const */ VISP_EXPORT bool inRectangle(const vpImagePoint &ip, const vpRect &rect) { - return (ip.get_i() <= rect.getBottom() && ip.get_i() >= rect.getTop() && ip.get_j() <= rect.getRight() && - ip.get_j() >= rect.getLeft()); + return ((ip.get_i() <= rect.getBottom()) && (ip.get_i() >= rect.getTop()) && (ip.get_j() <= rect.getRight()) && + (ip.get_j() >= rect.getLeft())); } VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpRect &r) diff --git a/modules/core/src/tools/geometry/vpRectOriented.cpp b/modules/core/src/tools/geometry/vpRectOriented.cpp index fae09c79e4..8f34891035 100644 --- a/modules/core/src/tools/geometry/vpRectOriented.cpp +++ b/modules/core/src/tools/geometry/vpRectOriented.cpp @@ -53,14 +53,14 @@ vpRectOriented::vpRectOriented(const vpImagePoint ¢er, double width, double m_width = width; m_height = height; m_theta = theta; - m_topLeft.set_i(m_center.get_i() - m_height * cos(m_theta) / 2.0 - m_width * sin(m_theta) / 2.0); - m_topLeft.set_j(m_center.get_j() + m_height * sin(m_theta) / 2.0 - m_width * cos(m_theta) / 2.0); - m_bottomLeft.set_i(m_center.get_i() + m_height * cos(m_theta) / 2.0 - m_width * sin(m_theta) / 2.0); - m_bottomLeft.set_j(m_center.get_j() - m_height * sin(m_theta) / 2.0 - m_width * cos(m_theta) / 2.0); - m_bottomRight.set_i(m_center.get_i() + m_height * cos(m_theta) / 2.0 + m_width * sin(m_theta) / 2.0); - m_bottomRight.set_j(m_center.get_j() - m_height * sin(m_theta) / 2.0 + m_width * cos(m_theta) / 2.0); - m_topRight.set_i(m_center.get_i() - m_height * cos(m_theta) / 2.0 + m_width * sin(m_theta) / 2.0); - m_topRight.set_j(m_center.get_j() + m_height * sin(m_theta) / 2.0 + m_width * cos(m_theta) / 2.0); + m_topLeft.set_i(m_center.get_i() - ((m_height * cos(m_theta)) / 2.0) - ((m_width * sin(m_theta)) / 2.0)); + m_topLeft.set_j(m_center.get_j() + ((m_height * sin(m_theta)) / 2.0) - ((m_width * cos(m_theta)) / 2.0)); + m_bottomLeft.set_i(m_center.get_i() + ((m_height * cos(m_theta)) / 2.0) - ((m_width * sin(m_theta)) / 2.0)); + m_bottomLeft.set_j(m_center.get_j() - ((m_height * sin(m_theta)) / 2.0) - ((m_width * cos(m_theta)) / 2.0)); + m_bottomRight.set_i(m_center.get_i() + ((m_height * cos(m_theta)) / 2.0) + ((m_width * sin(m_theta)) / 2.0)); + m_bottomRight.set_j(m_center.get_j() - ((m_height * sin(m_theta)) / 2.0) + ((m_width * cos(m_theta)) / 2.0)); + m_topRight.set_i(m_center.get_i() - ((m_height * cos(m_theta)) / 2.0) + ((m_width * sin(m_theta)) / 2.0)); + m_topRight.set_j(m_center.get_j() + ((m_height * sin(m_theta)) / 2.0) + ((m_width * cos(m_theta)) / 2.0)); } /** Copy constructor. @@ -72,14 +72,14 @@ vpRectOriented::vpRectOriented(const vpRect &rect) m_width = rect.getWidth(); m_height = rect.getHeight(); m_theta = .0; - m_topLeft.set_i(m_center.get_i() - m_height / 2.0); - m_topLeft.set_j(m_center.get_j() - m_width / 2.0); - m_bottomLeft.set_i(m_center.get_i() + m_height / 2.0); - m_bottomLeft.set_j(m_center.get_j() - m_width / 2.0); - m_bottomRight.set_i(m_center.get_i() + m_height / 2.0); - m_bottomRight.set_j(m_center.get_j() + m_width / 2.0); - m_topRight.set_i(m_center.get_i() - m_height / 2.0); - m_topRight.set_j(m_center.get_j() + m_width / 2.0); + m_topLeft.set_i(m_center.get_i() - (m_height / 2.0)); + m_topLeft.set_j(m_center.get_j() - (m_width / 2.0)); + m_bottomLeft.set_i(m_center.get_i() + (m_height / 2.0)); + m_bottomLeft.set_j(m_center.get_j() - (m_width / 2.0)); + m_bottomRight.set_i(m_center.get_i() + (m_height / 2.0)); + m_bottomRight.set_j(m_center.get_j() + (m_width / 2.0)); + m_topRight.set_i(m_center.get_i() - (m_height / 2.0)); + m_topRight.set_j(m_center.get_j() + (m_width / 2.0)); } #if (VISP_CXX_STANDARD == VISP_CXX_STANDARD_98) @@ -114,14 +114,14 @@ vpRectOriented &vpRectOriented::operator=(const vpRect &rect) m_width = rect.getWidth(); m_height = rect.getHeight(); m_theta = .0; - m_topLeft.set_i(m_center.get_i() - m_height / 2.0); - m_topLeft.set_j(m_center.get_j() - m_width / 2.0); - m_bottomLeft.set_i(m_center.get_i() + m_height / 2.0); - m_bottomLeft.set_j(m_center.get_j() - m_width / 2.0); - m_bottomRight.set_i(m_center.get_i() + m_height / 2.0); - m_bottomRight.set_j(m_center.get_j() + m_width / 2.0); - m_topRight.set_i(m_center.get_i() - m_height / 2.0); - m_topRight.set_j(m_center.get_j() + m_width / 2.0); + m_topLeft.set_i(m_center.get_i() - (m_height / 2.0)); + m_topLeft.set_j(m_center.get_j() - (m_width / 2.0)); + m_bottomLeft.set_i(m_center.get_i() + (m_height / 2.0)); + m_bottomLeft.set_j(m_center.get_j() - (m_width / 2.0)); + m_bottomRight.set_i(m_center.get_i() + (m_height / 2.0)); + m_bottomRight.set_j(m_center.get_j() + (m_width / 2.0)); + m_topRight.set_i(m_center.get_i() - (m_height / 2.0)); + m_topRight.set_j(m_center.get_j() + (m_width / 2.0)); return *this; } @@ -129,8 +129,9 @@ vpRectOriented &vpRectOriented::operator=(const vpRect &rect) */ vpRectOriented::operator vpRect() { - if (std::fabs(m_theta) > std::numeric_limits::epsilon()) + if (std::fabs(m_theta) > std::numeric_limits::epsilon()) { throw(vpException(vpException::badValue, "Cannot convert a vpRectOriented with non-zero orientation to a vpRect")); + } return vpRect(m_topLeft, m_bottomRight); } @@ -150,8 +151,8 @@ void vpRectOriented::setPoints(const vpImagePoint &topLeft, const vpImagePoint & m_center.set_j((m_topLeft.get_j() + m_bottomLeft.get_j() + m_bottomRight.get_j() + m_topRight.get_j()) / 4.0); m_width = sqrt((m_topRight.get_i() - m_topLeft.get_i()) * (m_topRight.get_i() - m_topLeft.get_i()) + (m_topRight.get_j() - m_topLeft.get_j()) * (m_topRight.get_j() - m_topLeft.get_j())); - m_height = sqrt((m_bottomLeft.get_i() - m_topLeft.get_i()) * (m_bottomLeft.get_i() - m_topLeft.get_i()) + - (m_bottomLeft.get_j() - m_topLeft.get_j()) * (m_bottomLeft.get_j() - m_topLeft.get_j())); + m_height = sqrt(((m_bottomLeft.get_i() - m_topLeft.get_i()) * (m_bottomLeft.get_i() - m_topLeft.get_i())) + + ((m_bottomLeft.get_j() - m_topLeft.get_j()) * (m_bottomLeft.get_j() - m_topLeft.get_j()))); m_theta = atan2(m_topRight.get_i() - m_topLeft.get_i(), m_topRight.get_j() - m_topLeft.get_j()); } @@ -185,14 +186,14 @@ void vpRectOriented::setSize(double width, double height) { m_width = width; m_height = height; - m_topLeft.set_i(m_center.get_i() - m_height * cos(m_theta) / 2.0 - m_width * sin(m_theta) / 2); - m_topLeft.set_j(m_center.get_j() + m_height * sin(m_theta) / 2.0 - m_width * cos(m_theta) / 2); - m_bottomLeft.set_i(m_center.get_i() + m_height * cos(m_theta) / 2.0 - m_width * sin(m_theta) / 2); - m_bottomLeft.set_j(m_center.get_j() - m_height * sin(m_theta) / 2.0 - m_width * cos(m_theta) / 2); - m_bottomRight.set_i(m_center.get_i() + m_height * cos(m_theta) / 2.0 + m_width * sin(m_theta) / 2); - m_bottomRight.set_j(m_center.get_j() - m_height * sin(m_theta) / 2.0 + m_width * cos(m_theta) / 2); - m_topRight.set_i(m_center.get_i() - m_height * cos(m_theta) / 2.0 + m_width * sin(m_theta) / 2); - m_topRight.set_j(m_center.get_j() + m_height * sin(m_theta) / 2.0 + m_width * cos(m_theta) / 2); + m_topLeft.set_i(m_center.get_i() - ((m_height * cos(m_theta)) / 2.0) - ((m_width * sin(m_theta)) / 2)); + m_topLeft.set_j(m_center.get_j() + ((m_height * sin(m_theta)) / 2.0) - ((m_width * cos(m_theta)) / 2)); + m_bottomLeft.set_i(m_center.get_i() + ((m_height * cos(m_theta)) / 2.0) - ((m_width * sin(m_theta)) / 2)); + m_bottomLeft.set_j(m_center.get_j() - ((m_height * sin(m_theta)) / 2.0) - ((m_width * cos(m_theta)) / 2)); + m_bottomRight.set_i(m_center.get_i() + ((m_height * cos(m_theta)) / 2.0) + ((m_width * sin(m_theta)) / 2)); + m_bottomRight.set_j(m_center.get_j() - ((m_height * sin(m_theta)) / 2.0) + ((m_width * cos(m_theta)) / 2)); + m_topRight.set_i(m_center.get_i() - ((m_height * cos(m_theta)) / 2.0) + ((m_width * sin(m_theta)) / 2)); + m_topRight.set_j(m_center.get_j() + ((m_height * sin(m_theta)) / 2.0) + ((m_width * cos(m_theta)) / 2)); } /// Get the rectangle width. @@ -205,14 +206,14 @@ double vpRectOriented::getHeight() const { return m_height; } void vpRectOriented::setOrientation(double theta) { m_theta = theta; - m_topLeft.set_i(m_center.get_i() - m_height * cos(m_theta) / 2.0 - m_width * sin(m_theta) / 2); - m_topLeft.set_j(m_center.get_j() + m_height * sin(m_theta) / 2.0 - m_width * cos(m_theta) / 2); - m_bottomLeft.set_i(m_center.get_i() + m_height * cos(m_theta) / 2.0 - m_width * sin(m_theta) / 2); - m_bottomLeft.set_j(m_center.get_j() - m_height * sin(m_theta) / 2.0 - m_width * cos(m_theta) / 2); - m_bottomRight.set_i(m_center.get_i() + m_height * cos(m_theta) / 2.0 + m_width * sin(m_theta) / 2); - m_bottomRight.set_j(m_center.get_j() - m_height * sin(m_theta) / 2.0 + m_width * cos(m_theta) / 2); - m_topRight.set_i(m_center.get_i() - m_height * cos(m_theta) / 2.0 + m_width * sin(m_theta) / 2); - m_topRight.set_j(m_center.get_j() + m_height * sin(m_theta) / 2.0 + m_width * cos(m_theta) / 2); + m_topLeft.set_i(m_center.get_i() - ((m_height * cos(m_theta)) / 2.0) - ((m_width * sin(m_theta)) / 2)); + m_topLeft.set_j(m_center.get_j() + ((m_height * sin(m_theta)) / 2.0) - ((m_width * cos(m_theta)) / 2)); + m_bottomLeft.set_i(m_center.get_i() + ((m_height * cos(m_theta)) / 2.0) - ((m_width * sin(m_theta)) / 2)); + m_bottomLeft.set_j(m_center.get_j() - ((m_height * sin(m_theta)) / 2.0) - ((m_width * cos(m_theta)) / 2)); + m_bottomRight.set_i(m_center.get_i() + ((m_height * cos(m_theta)) / 2.0) + ((m_width * sin(m_theta)) / 2)); + m_bottomRight.set_j(m_center.get_j() - ((m_height * sin(m_theta)) / 2.0) + ((m_width * cos(m_theta)) / 2)); + m_topRight.set_i(m_center.get_i() - ((m_height * cos(m_theta)) / 2.0) + ((m_width * sin(m_theta)) / 2)); + m_topRight.set_j(m_center.get_j() + ((m_height * sin(m_theta)) / 2.0) + ((m_width * cos(m_theta)) / 2)); } /// Get the rectangle orientation (rad). @@ -221,14 +222,18 @@ double vpRectOriented::getOrientation() const { return m_theta; } /// Check whether the point is inside the rectangle. bool vpRectOriented::isInside(const vpImagePoint &point) const { - if (!isLeft(point, m_topLeft, m_bottomLeft)) + if (!isLeft(point, m_topLeft, m_bottomLeft)) { return false; - if (!isLeft(point, m_bottomLeft, m_bottomRight)) + } + if (!isLeft(point, m_bottomLeft, m_bottomRight)) { return false; - if (!isLeft(point, m_bottomRight, m_topRight)) + } + if (!isLeft(point, m_bottomRight, m_topRight)) { return false; - if (!isLeft(point, m_topRight, m_topLeft)) + } + if (!isLeft(point, m_topRight, m_topLeft)) { return false; + } return true; } @@ -237,7 +242,7 @@ bool vpRectOriented::isLeft(const vpImagePoint &pointToTest, const vpImagePoint { double a = point1.get_j() - point2.get_j(); double b = point2.get_i() - point1.get_i(); - double c = -(a * point1.get_i() + b * point1.get_j()); - double d = a * pointToTest.get_i() + b * pointToTest.get_j() + c; + double c = -((a * point1.get_i()) + (b * point1.get_j())); + double d = (a * pointToTest.get_i()) + (b * pointToTest.get_j()) + c; return (d > 0); } diff --git a/modules/core/src/tools/geometry/vpXmlParserRectOriented.cpp b/modules/core/src/tools/geometry/vpXmlParserRectOriented.cpp index 2822e9ef75..8aca50972f 100644 --- a/modules/core/src/tools/geometry/vpXmlParserRectOriented.cpp +++ b/modules/core/src/tools/geometry/vpXmlParserRectOriented.cpp @@ -133,8 +133,9 @@ class vpXmlParserRectOriented::Impl root_node = doc.append_child("config"); } else if (!append) { - if (!vpIoTools::remove(filename)) + if (!vpIoTools::remove(filename)) { throw vpException(vpException::ioError, "Cannot remove existing xml file"); + } root_node = doc.append_child(pugi::node_declaration); root_node.append_attribute("version") = "1.0"; diff --git a/modules/core/src/tools/histogram/vpHistogram.cpp b/modules/core/src/tools/histogram/vpHistogram.cpp index f008b3ae3b..2d34a5770e 100644 --- a/modules/core/src/tools/histogram/vpHistogram.cpp +++ b/modules/core/src/tools/histogram/vpHistogram.cpp @@ -179,10 +179,12 @@ bool compare_vpHistogramPeak(vpHistogramPeak first, vpHistogramPeak second); // comparison, bool compare_vpHistogramPeak(vpHistogramPeak first, vpHistogramPeak second) { - if (first.getValue() > second.getValue()) + if (first.getValue() > second.getValue()) { return true; - else + } + else { return false; + } } /*! @@ -234,7 +236,7 @@ vpHistogram::vpHistogram(const vpImage &I, const vpImage *p vpHistogram::~vpHistogram() { if (m_histogram != nullptr) { - // vpTRACE("free: %p", &histogram); + // --comment: vpTRACE("free: %p", &histogram); delete[] m_histogram; m_histogram = nullptr; m_size = 0; @@ -283,7 +285,7 @@ void vpHistogram::init(unsigned size_) mp_mask = nullptr; m_total = 0; - // vpTRACE("alloc: %p", &histogram); + // --comment: vpTRACE("alloc: %p", &histogram); } /*! @@ -303,7 +305,7 @@ void vpHistogram::calculate(const vpImage &I, unsigned int nbins, } m_size = nbins > 256 ? 256 : (nbins > 0 ? nbins : 256); - if (nbins > 256 || nbins == 0) { + if ((nbins > 256) || (nbins == 0)) { std::cerr << "nbins=" << nbins << " , nbins should be between ]0 ; 256] ; use by default nbins=256" << std::endl; } m_histogram = new unsigned int[m_size]; @@ -318,13 +320,13 @@ void vpHistogram::calculate(const vpImage &I, unsigned int nbins, use_single_thread = (nbThreads == 0 || nbThreads == 1); #endif - if (!use_single_thread && I.getSize() <= nbThreads) { + if (!use_single_thread && (I.getSize() <= nbThreads)) { use_single_thread = true; } unsigned int lut[256]; - for (unsigned int i = 0; i < 256; i++) { - lut[i] = (unsigned int)(i * m_size / 256.0); + for (unsigned int i = 0; i < 256; ++i) { + lut[i] = static_cast(i * m_size / 256.0); } if (use_single_thread) { @@ -332,11 +334,11 @@ void vpHistogram::calculate(const vpImage &I, unsigned int nbins, const bool alwaysTrue = true; const bool *ptrMaskCurrent = &alwaysTrue; if (mp_mask) { - ptrMaskCurrent = (const bool *)mp_mask->bitmap; + ptrMaskCurrent = static_cast(mp_mask->bitmap); } unsigned int size_ = I.getWidth() * I.getHeight(); - unsigned char *ptrStart = (unsigned char *)I.bitmap; + unsigned char *ptrStart = static_cast(I.bitmap); unsigned char *ptrEnd = ptrStart + size_; unsigned char *ptrCurrent = ptrStart; @@ -362,7 +364,7 @@ void vpHistogram::calculate(const vpImage &I, unsigned int nbins, unsigned int step = image_size / nbThreads; unsigned int last_step = image_size - step * (nbThreads - 1); - for (unsigned int index = 0; index < nbThreads; index++) { + for (unsigned int index = 0; index < nbThreads; ++index) { unsigned int start_index = index * step; unsigned int end_index = (index + 1) * step; @@ -383,16 +385,16 @@ void vpHistogram::calculate(const vpImage &I, unsigned int nbins, threadpool.push_back(histogram_thread); } - for (size_t cpt = 0; cpt < threadpool.size(); cpt++) { + for (size_t cpt = 0; cpt < threadpool.size(); ++cpt) { // Wait until thread ends up threadpool[cpt]->join(); } m_total = 0; - for (unsigned int cpt1 = 0; cpt1 < m_size; cpt1++) { + for (unsigned int cpt1 = 0; cpt1 < m_size; ++cpt1) { unsigned int sum = 0; - for (size_t cpt2 = 0; cpt2 < histogramParams.size(); cpt2++) { + for (size_t cpt2 = 0; cpt2 < histogramParams.size(); ++cpt2) { sum += histogramParams[cpt2]->m_histogram[cpt1]; } @@ -401,11 +403,11 @@ void vpHistogram::calculate(const vpImage &I, unsigned int nbins, } // Delete - for (size_t cpt = 0; cpt < threadpool.size(); cpt++) { + for (size_t cpt = 0; cpt < threadpool.size(); ++cpt) { delete threadpool[cpt]; } - for (size_t cpt = 0; cpt < histogramParams.size(); cpt++) { + for (size_t cpt = 0; cpt < histogramParams.size(); ++cpt) { delete histogramParams[cpt]; } #endif @@ -423,15 +425,15 @@ void vpHistogram::equalize(const vpImage &I, vpImage::max(), maxValue = 0; cdf[0] = m_histogram[0]; - if (cdf[0] < cdfMin && cdf[0] > 0) { + if ((cdf[0] < cdfMin) && (cdf[0] > 0)) { cdfMin = cdf[0]; minValue = 0; } - for (unsigned int i = 1; i < 256; i++) { + for (unsigned int i = 1; i < 256; ++i) { cdf[i] = cdf[i - 1] + m_histogram[i]; - if (cdf[i] < cdfMin && cdf[i] > 0) { + if ((cdf[i] < cdfMin) && (cdf[i] > 0)) { cdfMin = cdf[i]; minValue = i; } @@ -450,8 +452,8 @@ void vpHistogram::equalize(const vpImage &I, vpImage(nbPixels - cdfMin) * 255.0); } Iout = I; @@ -474,14 +476,14 @@ void vpHistogram::display(const vpImage &I, const vpColor &color, { unsigned int width = I.getWidth(), height = I.getHeight(); // Minimal width and height are 36 px - if (width < 36 || height < 36) { + if ((width < 36) || (height < 36)) { std::cerr << "Image I must have at least width >= 36 && height >= 36 !" << std::endl; return; } unsigned int maxValue = maxValue_; if (maxValue == 0) { - for (unsigned int i = 0; i < m_size; i++) { + for (unsigned int i = 0; i < m_size; ++i) { if (m_histogram[i] > maxValue) { maxValue = m_histogram[i]; } @@ -493,10 +495,10 @@ void vpHistogram::display(const vpImage &I, const vpColor &color, } // Keep 12 free pixels at the top unsigned int max_height = height - 12; - double ratio_height = max_height / (double)maxValue; - double ratio_width = (width - 1) / (double)(m_size - 1.0); + double ratio_height = max_height / static_cast(maxValue); + double ratio_width = (width - 1) / static_cast(m_size - 1.0); - for (unsigned int i = 1; i < m_size; i++) { + for (unsigned int i = 1; i < m_size; ++i) { unsigned int value1 = m_histogram[i - 1]; unsigned int value2 = m_histogram[i]; @@ -534,15 +536,15 @@ void vpHistogram::smooth(unsigned int fsize) vpHistogram h; h = *this; - int hsize = (int)fsize / 2; // half filter size + int hsize = static_cast(fsize) / 2; // half filter size - for (unsigned i = 0; i < m_size; i++) { + for (unsigned i = 0; i < m_size; ++i) { unsigned int sum = 0; unsigned int nb = 0; - for (int j = -hsize; j <= hsize; j++) { + for (int j = -hsize; j <= hsize; ++j) { // exploitation of the overflow to detect negative value... - if (/*(i + j) >= 0 &&*/ (i + (unsigned int)j) < m_size) { - sum += h.m_histogram[i + (unsigned int)j]; + if (/*(i + j) >= 0 &&*/ (i + static_cast(j)) < m_size) { + sum += h.m_histogram[i + static_cast(j)]; nb++; } } @@ -583,14 +585,14 @@ unsigned vpHistogram::getPeaks(std::list &peaks) nbpeaks = 0; prev_slope = 1; - for (unsigned i = 0; i < m_size - 1; i++) { - int next_slope = (int)m_histogram[i + 1] - (int)m_histogram[i]; // Next histogram inclination + 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 ((prev_slope < 0) && (next_slope > 0) ) { - // sum_level += i; - // cpt ++; - // continue; - // } + // --comment: if ((prev_slope < 0) && (next_slope > 0) ) { + // --comment: sum_level += i; + // --comment: cpt ++; + // --comment: continue; + // --comment: } if ((prev_slope > 0) && (next_slope == 0)) { sum_level += i + 1; @@ -604,8 +606,8 @@ unsigned vpHistogram::getPeaks(std::list &peaks) cpt++; unsigned int level = sum_level / cpt; - p.set((unsigned char)level, m_histogram[level]); - // vpTRACE("add %d %d", p.getLevel(), p.getValue()); + p.set(static_cast(level), m_histogram[level]); + // --comment: vpTRACE("add %d %d", p.getLevel(), p.getValue()); peaks.push_back(p); nbpeaks++; @@ -616,7 +618,7 @@ unsigned vpHistogram::getPeaks(std::list &peaks) cpt = 0; } if (prev_slope > 0) { - p.set((unsigned char)m_size - 1u, m_histogram[m_size - 1]); + p.set(static_cast(m_size) - 1u, m_histogram[m_size - 1]); // vpTRACE("add %d %d", p.getLevel(), p.getValue()); peaks.push_back(p); nbpeaks++; @@ -717,22 +719,24 @@ bool vpHistogram::getPeaks(unsigned char dist, vpHistogramPeak &peakl, vpHistogr // Parse the histogram to get the local maxima nbpeaks = 0; prev_slope = 1; - for (unsigned i = 0; i < m_size - 1; i++) { - int next_slope = (int)m_histogram[i + 1] - (int)m_histogram[i]; // Next histogram inclination + 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) continue; // Peak detection - if ((prev_slope > 0) && (next_slope < 0)) - peak[nbpeaks++] = (unsigned char)i; + if ((prev_slope > 0) && (next_slope < 0)) { + peak[nbpeaks++] = static_cast(i); + } prev_slope = next_slope; } - if (prev_slope > 0) - peak[nbpeaks++] = (unsigned char)(m_size - 1); + if (prev_slope > 0) { + peak[nbpeaks++] = static_cast(m_size - 1); + } // Get the global maximum index_highest_peak = 0; - for (unsigned int i = 0; i < nbpeaks; i++) { + for (unsigned int i = 0; i < nbpeaks; ++i) { if (m_histogram[peak[i]] > m_histogram[peak[index_highest_peak]]) { index_highest_peak = i; } @@ -742,12 +746,14 @@ bool vpHistogram::getPeaks(unsigned char dist, vpHistogramPeak &peakl, vpHistogr index_second_peak = index_highest_peak; // Search second local maximum on the left of the global maximum - for (unsigned i = 0; i < index_highest_peak; i++) { - if (peak[index_highest_peak] - peak[i] > dist) { + for (unsigned i = 0; i < index_highest_peak; ++i) { + if ((peak[index_highest_peak] - peak[i]) > dist) { prof = 0; - for (int j = peak[i]; j <= peak[index_highest_peak] - dist; j++) - if ((m_histogram[peak[i]] - m_histogram[j]) > prof) + for (int j = peak[i]; j <= (peak[index_highest_peak] - dist); ++j) { + if ((m_histogram[peak[i]] - m_histogram[j]) > prof) { prof = m_histogram[peak[i]] - m_histogram[j]; + } + } if (prof > maxprof) { maxprof = prof; @@ -757,12 +763,14 @@ bool vpHistogram::getPeaks(unsigned char dist, vpHistogramPeak &peakl, vpHistogr } // Search second local maximum on the right of the global maximum - for (unsigned i = index_highest_peak + 1; i < nbpeaks; i++) { - if (peak[i] - peak[index_highest_peak] > dist) { + for (unsigned i = index_highest_peak + 1; i < nbpeaks; ++i) { + if ((peak[i] - peak[index_highest_peak]) > dist) { prof = 0; - for (int j = peak[index_highest_peak] + dist; j <= peak[i]; j++) - if ((m_histogram[peak[i]] - m_histogram[j]) > prof) + for (int j = peak[index_highest_peak] + dist; j <= peak[i]; ++j) { + if ((m_histogram[peak[i]] - m_histogram[j]) > prof) { prof = m_histogram[peak[i]] - m_histogram[j]; + } + } if (prof > maxprof) { maxprof = prof; @@ -785,14 +793,15 @@ bool vpHistogram::getPeaks(unsigned char dist, vpHistogramPeak &peakl, vpHistogr delete[] peak; - return (false); // Not a bimodal histogram + return false; // Not a bimodal histogram } // Search the valey mini = peakl.getValue(); sumindmini = 0; nbmini = 0; - for (unsigned i = peakl.getLevel(); i <= peakr.getLevel(); i++) { + unsigned peakr_level = peakr.getLevel(); + for (unsigned i = peakl.getLevel(); i <= peakr_level; i++) { if (m_histogram[i] < mini) { mini = m_histogram[i]; nbmini = 1; @@ -804,9 +813,9 @@ bool vpHistogram::getPeaks(unsigned char dist, vpHistogramPeak &peakl, vpHistogr nbmini++; } } - // vpTRACE("nbmini %d", nbmini); - // vpTRACE("sumindmini %d", sumindmini); - // vpTRACE("mini: indmini: %d", sumindmini/nbmini); + // --comment: vpTRACE("nbmini %d", nbmini); + // --comment: vpTRACE("sumindmini %d", sumindmini); + // --comment: vpTRACE("mini: indmini: %d", sumindmini/nbmini); if (nbmini == 0) { // no valey found @@ -818,11 +827,11 @@ bool vpHistogram::getPeaks(unsigned char dist, vpHistogramPeak &peakl, vpHistogr } else { mini = sumindmini / nbmini; // mean - valey.set((unsigned char)mini, m_histogram[mini]); + valey.set(static_cast(mini), m_histogram[mini]); delete[] peak; - return (true); + return true; } } @@ -856,8 +865,8 @@ unsigned vpHistogram::getValey(std::list &valey) nbvaley = 0; prev_slope = -1; - for (unsigned i = 0; i < m_size - 1; i++) { - int next_slope = (int)m_histogram[i + 1] - (int)m_histogram[i]; // Next histogram inclination + 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 ((prev_slope < 0) && (next_slope == 0)) { sum_level += i + 1; @@ -871,7 +880,7 @@ unsigned vpHistogram::getValey(std::list &valey) cpt++; unsigned int level = sum_level / cpt; - p.set((unsigned char)level, m_histogram[level]); + p.set(static_cast(level), m_histogram[level]); // vpTRACE("add %d %d", p.getLevel(), p.getValue()); valey.push_back(p); @@ -883,7 +892,7 @@ unsigned vpHistogram::getValey(std::list &valey) cpt = 0; } if (prev_slope < 0) { - p.set((unsigned char)m_size - 1u, m_histogram[m_size - 1]); + p.set(static_cast(m_size) - 1u, m_histogram[m_size - 1]); // vpTRACE("add %d %d", p.getLevel(), p.getValue()); valey.push_back(p); nbvaley++; @@ -928,7 +937,8 @@ bool vpHistogram::getValey(const vpHistogramPeak &peak1, const vpHistogramPeak & mini = peakl.getValue(); sumindmini = 0; nbmini = 0; - for (unsigned i = peakl.getLevel(); i <= peakr.getLevel(); i++) { + unsigned peakr_level = peakr.getLevel(); + for (unsigned i = peakl.getLevel(); i <= peakr_level; i++) { if (m_histogram[i] < mini) { mini = m_histogram[i]; nbmini = 1; @@ -950,7 +960,7 @@ bool vpHistogram::getValey(const vpHistogramPeak &peak1, const vpHistogramPeak & else { unsigned int minipos = sumindmini / nbmini; // position of the minimum - valey.set((unsigned char)minipos, m_histogram[minipos]); + valey.set(static_cast(minipos), m_histogram[minipos]); return true; } } @@ -988,8 +998,8 @@ unsigned vpHistogram::getValey(unsigned char dist, const vpHistogramPeak &peak, valeyl.set(0, 0); ret &= 0x01; } - if (peak.getLevel() == m_size - 1) { - valeyr.set((unsigned char)(m_size - 1), 0); + if (peak.getLevel() == (m_size - 1)) { + valeyr.set(static_cast(m_size - 1), 0); ret &= 0x10; } @@ -1029,14 +1039,16 @@ unsigned vpHistogram::getValey(unsigned char dist, const vpHistogramPeak &peak, } } } - if (!found) + if (!found) { peakl.set(0, 0); + } // Search the valey on the left mini = peak.getValue(); sumindmini = 0; nbmini = 0; - for (unsigned i = peakl.getLevel(); i < peak.getLevel(); i++) { + unsigned peak_level = peak.getLevel(); + for (unsigned i = peakl.getLevel(); i < peak_level; ++i) { if (m_histogram[i] < mini) { mini = m_histogram[i]; nbmini = 1; @@ -1054,7 +1066,7 @@ unsigned vpHistogram::getValey(unsigned char dist, const vpHistogramPeak &peak, } else { unsigned int minipos = sumindmini / nbmini; // position of the minimum - valeyl.set((unsigned char)minipos, m_histogram[minipos]); + valeyl.set(static_cast(minipos), m_histogram[minipos]); ret &= 0x11; } } @@ -1085,14 +1097,15 @@ unsigned vpHistogram::getValey(unsigned char dist, const vpHistogramPeak &peak, break; // we can stop here } - if (!found) - peakr.set((unsigned char)(m_size - 1), 0); + if (!found) { + peakr.set(static_cast(m_size - 1), 0); + } // Search the valey on the right mini = peak.getValue(); sumindmini = 0; nbmini = 0; - for (unsigned i = (unsigned int)peak.getLevel() + 1; i <= (unsigned int)peakr.getLevel(); i++) { + for (unsigned i = static_cast(peak.getLevel()) + 1; i <= static_cast(peakr.getLevel()); ++i) { if (m_histogram[i] < mini) { mini = m_histogram[i]; nbmini = 1; @@ -1105,12 +1118,12 @@ unsigned vpHistogram::getValey(unsigned char dist, const vpHistogramPeak &peak, } } if (nbmini == 0) { - valeyr.set((unsigned char)(m_size - 1), 0); + valeyr.set(static_cast(m_size - 1), 0); ret &= 0x10; } else { unsigned int minipos = sumindmini / nbmini; // position of the minimum - valeyr.set((unsigned char)minipos, m_histogram[minipos]); + valeyr.set(static_cast(minipos), m_histogram[minipos]); ret &= 0x11; } } @@ -1135,7 +1148,7 @@ unsigned vpHistogram::sort(std::list &peaks) peaks.sort(compare_vpHistogramPeak); - return (unsigned int)peaks.size(); + return static_cast(peaks.size()); } /*! @@ -1167,10 +1180,12 @@ bool vpHistogram::write(const std::string &filename) { return (this->write(filen bool vpHistogram::write(const char *filename) { FILE *fd = fopen(filename, "w"); - if (fd == nullptr) + if (fd == nullptr) { return false; - for (unsigned i = 0; i < m_size; i++) + } + for (unsigned i = 0; i < m_size; ++i) { fprintf(fd, "%u %u\n", i, m_histogram[i]); + } fclose(fd); return true; diff --git a/modules/core/src/tools/histogram/vpHistogramPeak.cpp b/modules/core/src/tools/histogram/vpHistogramPeak.cpp index 6b507e8490..42804768f6 100644 --- a/modules/core/src/tools/histogram/vpHistogramPeak.cpp +++ b/modules/core/src/tools/histogram/vpHistogramPeak.cpp @@ -92,7 +92,7 @@ bool vpHistogramPeak::operator==(const vpHistogramPeak &p) const { return ((leve VISP_EXPORT std::ostream &operator<<(std::ostream &s, const vpHistogramPeak &p) { - s << (int)p.getLevel() << " " << p.getValue(); + s << static_cast(p.getLevel()) << " " << p.getValue(); return s; } diff --git a/modules/core/src/tools/histogram/vpHistogramValey.cpp b/modules/core/src/tools/histogram/vpHistogramValey.cpp index 65d26899c3..2a0f14f78f 100644 --- a/modules/core/src/tools/histogram/vpHistogramValey.cpp +++ b/modules/core/src/tools/histogram/vpHistogramValey.cpp @@ -80,7 +80,7 @@ bool vpHistogramValey::operator==(const vpHistogramValey &v) const VISP_EXPORT std::ostream &operator<<(std::ostream &s, const vpHistogramValey &v) { - s << (int)v.getLevel() << " " << v.getValue(); + s << static_cast(v.getLevel()) << " " << v.getValue(); return s; } diff --git a/modules/core/src/tools/time/vpTime.cpp b/modules/core/src/tools/time/vpTime.cpp index fd78ac22fa..90481258f0 100644 --- a/modules/core/src/tools/time/vpTime.cpp +++ b/modules/core/src/tools/time/vpTime.cpp @@ -174,10 +174,11 @@ int wait(double t0, double t) double timeCurrent, timeToWait; timeCurrent = measureTimeMs(); - timeToWait = t0 + t - timeCurrent; + timeToWait = t0 + (t - timeCurrent); - if (timeToWait <= 0.) // no need to wait + if (timeToWait <= 0.) { // no need to wait return (1); + } else { #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX if (timeToWait > vpTime::minTimeForUsleepCall) { @@ -186,7 +187,7 @@ int wait(double t0, double t) // Blocking loop to have an accurate waiting do { timeCurrent = measureTimeMs(); - timeToWait = t0 + t - timeCurrent; + timeToWait = t0 + (t - timeCurrent); } while (timeToWait > 0.); @@ -224,8 +225,9 @@ void wait(double t) { double timeToWait = t; - if (timeToWait <= 0.) // no need to wait + if (timeToWait <= 0.) { // no need to wait return; + } else { #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX double t0 = measureTimeMs(); @@ -235,7 +237,7 @@ void wait(double t) // Blocking loop to have an accurate waiting do { double timeCurrent = measureTimeMs(); - timeToWait = t0 + t - timeCurrent; + timeToWait = t0 + (t - timeCurrent); } while (timeToWait > 0.); diff --git a/modules/core/src/tracking/forward-projection/vpCircle.cpp b/modules/core/src/tracking/forward-projection/vpCircle.cpp index 58862de32a..35d0f9d0c6 100644 --- a/modules/core/src/tracking/forward-projection/vpCircle.cpp +++ b/modules/core/src/tracking/forward-projection/vpCircle.cpp @@ -160,62 +160,62 @@ void vpCircle::projection(const vpColVector &cP_, vpColVector &p_) const p_.resize(5, false); vpColVector K(6); - { - double A = cP_[0]; - double B = cP_[1]; - double C = cP_[2]; - - double X0 = cP_[3]; - double Y0 = cP_[4]; - double Z0 = cP_[5]; - - double r = cP_[6]; - - // projection - double s = X0 * X0 + Y0 * Y0 + Z0 * Z0 - r * r; - double det = A * X0 + B * Y0 + C * Z0; - A = A / det; - B = B / det; - C = C / det; - - K[0] = 1 - 2 * A * X0 + A * A * s; - K[1] = 1 - 2 * B * Y0 + B * B * s; - K[2] = -A * Y0 - B * X0 + A * B * s; - K[3] = -C * X0 - A * Z0 + A * C * s; - K[4] = -C * Y0 - B * Z0 + B * C * s; - K[5] = 1 - 2 * C * Z0 + C * C * s; - } +{ + double A = cP_[0]; + double B = cP_[1]; + double C = cP_[2]; + + double X0 = cP_[3]; + double Y0 = cP_[4]; + double Z0 = cP_[5]; + + double r = cP_[6]; + + // projection + double s = (X0 * X0) + (Y0 * Y0) + (Z0 * Z0) - (r * r); + double det = (A * X0) + (B * Y0) + (C * Z0); + A = A / det; + B = B / det; + C = C / det; + + K[0] = (1 - (2 * A * X0)) + (A * A * s); + K[1] = (1 - (2 * B * Y0)) + (B * B * s); + K[2] = ((-A * Y0) - (B * X0)) + (A * B * s); + K[3] = ((-C * X0) - (A * Z0)) + (A * C * s); + K[4] = ((-C * Y0) - (B * Z0)) + (B * C * s); + K[5] = (1 - (2 * C * Z0)) + (C * C * s); +} - double det = K[2] * K[2] - K[0] * K[1]; + double det = (K[2] * K[2]) - (K[0] * K[1]); if (fabs(det) < det_threshold) { throw(vpException(vpException::divideByZeroError, "Division by 0 in vpCircle::projection.")); } - double xc = (K[1] * K[3] - K[2] * K[4]) / det; - double yc = (K[0] * K[4] - K[2] * K[3]) / det; + double xc = ((K[1] * K[3]) - (K[2] * K[4])) / det; + double yc = ((K[0] * K[4]) - (K[2] * K[3])) / det; - double c = sqrt((K[0] - K[1]) * (K[0] - K[1]) + 4 * K[2] * K[2]); - double s = 2 * (K[0] * xc * xc + 2 * K[2] * xc * yc + K[1] * yc * yc - K[5]); + double c = sqrt(((K[0] - K[1]) * (K[0] - K[1])) + (4 * K[2] * K[2])); + double s = 2 * (((K[0] * xc * xc) + (2 * K[2] * xc * yc) + (K[1] * yc * yc)) - K[5]); double A, B, E; if (fabs(K[2]) < std::numeric_limits::epsilon()) { E = 0.0; if (K[0] > K[1]) { - A = sqrt(s / (K[0] + K[1] + c)); - B = sqrt(s / (K[0] + K[1] - c)); + A = sqrt(s / ((K[0] + K[1]) + c)); + B = sqrt(s / ((K[0] + K[1]) - c)); } else { - A = sqrt(s / (K[0] + K[1] - c)); - B = sqrt(s / (K[0] + K[1] + c)); + A = sqrt(s / ((K[0] + K[1]) - c)); + B = sqrt(s / ((K[0] + K[1]) + c)); } } else { - E = (K[1] - K[0] + c) / (2 * K[2]); + E = ( (K[1] - K[0]) + c) / (2 * K[2]); if (fabs(E) > 1.0) { - A = sqrt(s / (K[0] + K[1] + c)); - B = sqrt(s / (K[0] + K[1] - c)); + A = sqrt(s / ((K[0] + K[1]) + c)); + B = sqrt(s / ((K[0] + K[1]) - c)); } else { - A = sqrt(s / (K[0] + K[1] - c)); - B = sqrt(s / (K[0] + K[1] + c)); + A = sqrt(s / ((K[0] + K[1]) - c)); + B = sqrt(s / ((K[0] + K[1]) + c)); E = -1.0 / E; } } @@ -223,7 +223,7 @@ void vpCircle::projection(const vpColVector &cP_, vpColVector &p_) const // Chaumette PhD Thesis 1990, eq 2.72 divided by 4 since n_ij = mu_ij_chaumette_thesis / 4 det = 4 * (1.0 + vpMath::sqr(E)); double n20 = (vpMath::sqr(A) + vpMath::sqr(B * E)) / det; - double n11 = (vpMath::sqr(A) - vpMath::sqr(B)) * E / det; + double n11 = ((vpMath::sqr(A) - vpMath::sqr(B)) * E) / det; double n02 = (vpMath::sqr(B) + vpMath::sqr(A * E)) / det; p_[0] = xc; @@ -248,14 +248,14 @@ void vpCircle::changeFrame(const vpHomogeneousMatrix &noMo, vpColVector &noP) co noP.resize(7, false); double A, B, C; - A = noMo[0][0] * oP[0] + noMo[0][1] * oP[1] + noMo[0][2] * oP[2]; - B = noMo[1][0] * oP[0] + noMo[1][1] * oP[1] + noMo[1][2] * oP[2]; - C = noMo[2][0] * oP[0] + noMo[2][1] * oP[1] + noMo[2][2] * oP[2]; + A = (noMo[0][0] * oP[0]) + (noMo[0][1] * oP[1]) + (noMo[0][2] * oP[2]); + B = (noMo[1][0] * oP[0]) + (noMo[1][1] * oP[1]) + (noMo[1][2] * oP[2]); + C = (noMo[2][0] * oP[0]) + (noMo[2][1] * oP[1]) + (noMo[2][2] * oP[2]); double X0, Y0, Z0; - X0 = noMo[0][3] + noMo[0][0] * oP[3] + noMo[0][1] * oP[4] + noMo[0][2] * oP[5]; - Y0 = noMo[1][3] + noMo[1][0] * oP[3] + noMo[1][1] * oP[4] + noMo[1][2] * oP[5]; - Z0 = noMo[2][3] + noMo[2][0] * oP[3] + noMo[2][1] * oP[4] + noMo[2][2] * oP[5]; + X0 = noMo[0][3] + (noMo[0][0] * oP[3]) + (noMo[0][1] * oP[4]) + (noMo[0][2] * oP[5]); + Y0 = noMo[1][3] + (noMo[1][0] * oP[3]) + (noMo[1][1] * oP[4]) + (noMo[1][2] * oP[5]); + Z0 = noMo[2][3] + (noMo[2][0] * oP[3]) + (noMo[2][1] * oP[4]) + (noMo[2][2] * oP[5]); double R = oP[6]; noP[0] = A; @@ -278,14 +278,14 @@ void vpCircle::changeFrame(const vpHomogeneousMatrix &noMo, vpColVector &noP) co void vpCircle::changeFrame(const vpHomogeneousMatrix &cMo) { double A, B, C; - A = cMo[0][0] * oP[0] + cMo[0][1] * oP[1] + cMo[0][2] * oP[2]; - B = cMo[1][0] * oP[0] + cMo[1][1] * oP[1] + cMo[1][2] * oP[2]; - C = cMo[2][0] * oP[0] + cMo[2][1] * oP[1] + cMo[2][2] * oP[2]; + A = (cMo[0][0] * oP[0]) + (cMo[0][1] * oP[1]) + (cMo[0][2] * oP[2]); + B = (cMo[1][0] * oP[0]) + (cMo[1][1] * oP[1]) + (cMo[1][2] * oP[2]); + C = (cMo[2][0] * oP[0]) + (cMo[2][1] * oP[1]) + (cMo[2][2] * oP[2]); double X0, Y0, Z0; - X0 = cMo[0][3] + cMo[0][0] * oP[3] + cMo[0][1] * oP[4] + cMo[0][2] * oP[5]; - Y0 = cMo[1][3] + cMo[1][0] * oP[3] + cMo[1][1] * oP[4] + cMo[1][2] * oP[5]; - Z0 = cMo[2][3] + cMo[2][0] * oP[3] + cMo[2][1] * oP[4] + cMo[2][2] * oP[5]; + X0 = cMo[0][3] + (cMo[0][0] * oP[3]) + (cMo[0][1] * oP[4]) + (cMo[0][2] * oP[5]); + Y0 = cMo[1][3] + (cMo[1][0] * oP[3]) + (cMo[1][1] * oP[4]) + (cMo[1][2] * oP[5]); + Z0 = cMo[2][3] + (cMo[2][0] * oP[3]) + (cMo[2][1] * oP[4]) + (cMo[2][2] * oP[5]); double R = oP[6]; cP[0] = A; @@ -343,10 +343,10 @@ void vpCircle::display(const vpImage &I, const vpCameraParameters &cam, void vpCircle::display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &color, unsigned int thickness) { - vpColVector _cP, _p; - changeFrame(cMo, _cP); - projection(_cP, _p); - vpFeatureDisplay::displayEllipse(_p[0], _p[1], _p[2], _p[3], _p[4], cam, I, color, thickness); + vpColVector v_cP, v_p; + changeFrame(cMo, v_cP); + projection(v_cP, v_p); + vpFeatureDisplay::displayEllipse(v_p[0], v_p[1], v_p[2], v_p[3], v_p[4], cam, I, color, thickness); } /*! @@ -405,8 +405,8 @@ void vpCircle::computeIntersectionPoint(const vpCircle &circle, const vpCameraPa double n11 = circle.p[3]; double n02 = circle.p[4]; double n20 = circle.p[2]; - double Xg = u0 + circle.p[0] * px; - double Yg = v0 + circle.p[1] * py; + double Xg = u0 + (circle.p[0] * px); + double Yg = v0 + (circle.p[1] * py); // Find Intersection between line and ellipse in the image. @@ -418,14 +418,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/core/src/tracking/forward-projection/vpLine.cpp b/modules/core/src/tracking/forward-projection/vpLine.cpp index 1334850f63..45460e7bf3 100644 --- a/modules/core/src/tracking/forward-projection/vpLine.cpp +++ b/modules/core/src/tracking/forward-projection/vpLine.cpp @@ -113,9 +113,9 @@ void vpLine::setWorldCoordinates(const double &oA1, const double &oB1, const dou */ void vpLine::setWorldCoordinates(const vpColVector &oP_) { - if (oP_.getRows() != 8) + if (oP_.getRows() != 8) { throw vpException(vpException::dimensionError, "Size of oP is not equal to 8 as it should be"); - + } this->oP = oP_; } @@ -142,13 +142,13 @@ void vpLine::setWorldCoordinates(const vpColVector &oP_) */ void vpLine::setWorldCoordinates(const vpColVector &oP1, const vpColVector &oP2) { - if (oP1.getRows() != 4) + if (oP1.getRows() != 4) { throw vpException(vpException::dimensionError, "Size of oP1 is not equal to 4 as it should be"); - - if (oP2.getRows() != 4) + } + if (oP2.getRows() != 4) { throw vpException(vpException::dimensionError, "Size of oP2 is not equal to 4 as it should be"); - - for (unsigned int i = 0; i < 4; i++) { + } + for (unsigned int i = 0; i < 4; ++i) { oP[i] = oP1[i]; oP[i + 4] = oP2[i]; } @@ -212,9 +212,9 @@ void vpLine::projection(const vpColVector &cP_, vpColVector &p_) const p_.resize(2, false); // projection - if (cP.getRows() != 8) + if (cP.getRows() != 8) { throw vpException(vpException::dimensionError, "Size of cP is not equal to 8 as it should be"); - + } double A1, A2, B1, B2, C1, C2, D1, D2; A1 = cP_[0]; @@ -228,10 +228,10 @@ void vpLine::projection(const vpColVector &cP_, vpColVector &p_) const D2 = cP_[7]; double a, b, c, s; - a = A2 * D1 - A1 * D2; - b = B2 * D1 - B1 * D2; - c = C2 * D1 - C1 * D2; - s = a * a + b * b; + a = (A2 * D1) - (A1 * D2); + b = (B2 * D1) - (B1 * D2); + c = (C2 * D1) - (C1 * D2); + s = (a * a) + (b * b); if (s <= 1e-8) // seuil pas terrible { printf("Degenerate case: the image of the straight line is a point!\n"); @@ -344,19 +344,19 @@ void vpLine::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_) const c2 = oP[6]; d2 = oP[7]; - A1 = cMo[0][0] * a1 + cMo[0][1] * b1 + cMo[0][2] * c1; - B1 = cMo[1][0] * a1 + cMo[1][1] * b1 + cMo[1][2] * c1; - C1 = cMo[2][0] * a1 + cMo[2][1] * b1 + cMo[2][2] * c1; - D1 = d1 - (cMo[0][3] * A1 + cMo[1][3] * B1 + cMo[2][3] * C1); + A1 = (cMo[0][0] * a1) + (cMo[0][1] * b1) + (cMo[0][2] * c1); + B1 = (cMo[1][0] * a1) + (cMo[1][1] * b1) + (cMo[1][2] * c1); + C1 = (cMo[2][0] * a1) + (cMo[2][1] * b1) + (cMo[2][2] * c1); + D1 = d1 - ((cMo[0][3] * A1) + (cMo[1][3] * B1) + (cMo[2][3] * C1)); - A2 = cMo[0][0] * a2 + cMo[0][1] * b2 + cMo[0][2] * c2; - B2 = cMo[1][0] * a2 + cMo[1][1] * b2 + cMo[1][2] * c2; - C2 = cMo[2][0] * a2 + cMo[2][1] * b2 + cMo[2][2] * c2; - D2 = d2 - (cMo[0][3] * A2 + cMo[1][3] * B2 + cMo[2][3] * C2); + A2 = (cMo[0][0] * a2) + (cMo[0][1] * b2) + (cMo[0][2] * c2); + B2 = (cMo[1][0] * a2) + (cMo[1][1] * b2) + (cMo[1][2] * c2); + C2 = (cMo[2][0] * a2) + (cMo[2][1] * b2) + (cMo[2][2] * c2); + D2 = d2 - ((cMo[0][3] * A2) + (cMo[1][3] * B2) + (cMo[2][3] * C2)); // in case of verification - // ap1 = A1; bp1 = B1; cp1 = C1; dp1 = D1; - // ap2 = A2; bp2 = B2; cp2 = C2; dp2 = D2; + // --comment: ap1 = A1; bp1 = B1; cp1 = C1; dp1 = D1 + // --comment: ap2 = A2; bp2 = B2; cp2 = C2; dp2 = D2 // vpERROR_TRACE("A1 B1 C1 D1 %f %f %f %f ", A1, B1, C1, D1) ; // vpERROR_TRACE("A2 B2 C2 D2 %f %f %f %f ", A2, B2, C2, D2) ; @@ -364,14 +364,14 @@ void vpLine::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_) const // Adding constraints on the straight line to have a unique representation // direction of the straight line = N1 x N2 - a2 = B1 * C2 - C1 * B2; - b2 = C1 * A2 - A1 * C2; - c2 = A1 * B2 - B1 * A2; + a2 = (B1 * C2) - (C1 * B2); + b2 = (C1 * A2) - (A1 * C2); + c2 = (A1 * B2) - (B1 * A2); // Constraint D1 = 0 (the origin belongs to P1) - a1 = A2 * D1 - A1 * D2; - b1 = B2 * D1 - B1 * D2; - c1 = C2 * D1 - C1 * D2; + a1 = (A2 * D1) - (A1 * D2); + b1 = (B2 * D1) - (B1 * D2); + c1 = (C2 * D1) - (C1 * D2); if (fabs(D2) < fabs(D1)) // to be sure that D2 <> 0 { @@ -382,26 +382,30 @@ void vpLine::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_) const } // Constraint A1^2 + B1^2 + C1^2 = 1 - d1 = 1.0 / sqrt(a1 * a1 + b1 * b1 + c1 * c1); - cP_[0] = A1 = a1 * d1; - cP_[1] = B1 = b1 * d1; - cP_[2] = C1 = c1 * d1; + d1 = 1.0 / sqrt((a1 * a1) + (b1 * b1) + (c1 * c1)); + A1 = a1 * d1; + B1 = b1 * d1; + C1 = c1 * d1; + cP_[0] = A1; + cP_[1] = B1; + cP_[2] = C1; + cP_[3] = 0; // Constraint A1 A2 + B1 B2 + C1 C2 = 0 (P2 orthogonal to P1) // N2_new = (N1 x N2) x N1_new - a1 = b2 * C1 - c2 * B1; - b1 = c2 * A1 - a2 * C1; - c1 = a2 * B1 - b2 * A1; + a1 = (b2 * C1) - (c2 * B1); + b1 = (c2 * A1) - (a2 * C1); + c1 = (a2 * B1) - (b2 * A1); // Constraint A2^2 + B2^2 + C2^2 = 1 - d1 = 1.0 / sqrt(a1 * a1 + b1 * b1 + c1 * c1); + d1 = 1.0 / sqrt((a1 * a1) + (b1 * b1) + (c1 * c1)); a1 *= d1; b1 *= d1; c1 *= d1; // D2_new = D2 / (N2^T . N2_new) - D2 /= (A2 * a1 + B2 * b1 + C2 * c1); + D2 /= ((A2 * a1) + (B2 * b1) + (C2 * c1)); A2 = a1; B2 = b1; C2 = c1; @@ -420,27 +424,6 @@ void vpLine::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_) const cP_[5] = B2; cP_[6] = C2; cP_[7] = D2; - - // in case of verification - /* - x = -A2*D2; - y = -B2*D2; - z = -C2*D2; - d1 = ap1*x+bp1*y+cp1*z+dp1; - d2 = ap2*x+bp2*y+cp2*z+dp2; - if ((fabs(d1) > 1e-8) || (fabs(d2) > 1e-8)) - { - printf("PB in VPline: P1 : 0 = %lf, P2: 0 = %lf\n",d1,d2); - return EXIT_FAILURE; - } - d1 = A1*x+B1*y+C1*z+D1; - d2 = A2*x+B2*y+C2*z+D2; - if ((fabs(d1) > 1e-8) || (fabs(d2) > 1e-8)) - { - printf("PB in VPline: Pn1 : 0 = %lf, Pn2: 0 = %lf\n",d1,d2); - return EXIT_FAILURE; - } - */ } /*! @@ -511,13 +494,14 @@ void vpLine::display(const vpImage &I, const vpCameraParameters &cam, co void vpLine::display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &color, unsigned int thickness) { - vpColVector _cP, _p; - changeFrame(cMo, _cP); + vpColVector v_cP, v_p; + changeFrame(cMo, v_cP); try { - projection(_cP, _p); - vpFeatureDisplay::displayLine(_p[0], _p[1], cam, I, color, thickness); - } catch (...) { - // Skip potential exception: due to a degenerate case: the image of the straight line is a point! + projection(v_cP, v_p); + vpFeatureDisplay::displayLine(v_p[0], v_p[1], cam, I, color, thickness); + } + catch (...) { + // Skip potential exception: due to a degenerate case: the image of the straight line is a point! } } @@ -546,13 +530,14 @@ void vpLine::display(const vpImage &I, const vpHomogeneousMatrix void vpLine::display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &color, unsigned int thickness) { - vpColVector _cP, _p; - changeFrame(cMo, _cP); + vpColVector v_cP, v_p; + changeFrame(cMo, v_cP); try { - projection(_cP, _p); - vpFeatureDisplay::displayLine(_p[0], _p[1], cam, I, color, thickness); - } catch (...) { - // Skip potential exception: due to a degenerate case: the image of the straight line is a point! + projection(v_cP, v_p); + vpFeatureDisplay::displayLine(v_p[0], v_p[1], cam, I, color, thickness); + } + catch (...) { + // Skip potential exception: due to a degenerate case: the image of the straight line is a point! } } diff --git a/modules/core/src/tracking/forward-projection/vpPoint.cpp b/modules/core/src/tracking/forward-projection/vpPoint.cpp index 50e439619b..2767575b43 100644 --- a/modules/core/src/tracking/forward-projection/vpPoint.cpp +++ b/modules/core/src/tracking/forward-projection/vpPoint.cpp @@ -200,8 +200,10 @@ void vpPoint::getWorldCoordinates(vpColVector &oP_) { oP_ = oP; } void vpPoint::getWorldCoordinates(std::vector &oP_) { oP_.resize(oP.size()); - for (unsigned int i = 0; i < oP.size(); i++) + unsigned int oP_size = oP.size(); + for (unsigned int i = 0; i < oP_size; ++i) { oP_[i] = oP[i]; + } } /*! @@ -215,18 +217,18 @@ vpColVector vpPoint::getWorldCoordinates(void) { return this->oP; } /*! Compute the perspective projection of a point _cP. - \param _cP : 3-dim vector cP = (cX, cY, cZ) or 4-dim vector cP = (cX, cY, cZ, 1) corresponding + \param v_cP : 3-dim vector cP = (cX, cY, cZ) or 4-dim vector cP = (cX, cY, cZ, 1) corresponding to the normalized coordinates of the 3D point in the camera frame. - \param _p : Coordinates of the point in the + \param v_p : Coordinates of the point in the image plane obtained by perspective projection. */ -void vpPoint::projection(const vpColVector &_cP, vpColVector &_p) const +void vpPoint::projection(const vpColVector &v_cP, vpColVector &v_p) const { - _p.resize(3, false); + v_p.resize(3, false); - _p[0] = _cP[0] / _cP[2]; - _p[1] = _cP[1] / _cP[2]; - _p[2] = 1; + v_p[0] = v_cP[0] / v_cP[2]; + v_p[1] = v_cP[1] / v_cP[2]; + v_p[2] = 1; } /*! @@ -235,22 +237,22 @@ void vpPoint::projection(const vpColVector &_cP, vpColVector &_p) const 3D coordinates of the point in the camera frame cP = cMo * oP. \param cMo : Transformation from camera to object frame. - \param _cP : 3D normalized coordinates of the point in the camera frame cP = (cX, cY, cZ, 1). + \param v_cP : 3D normalized coordinates of the point in the camera frame cP = (cX, cY, cZ, 1). */ -void vpPoint::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &_cP) const +void vpPoint::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &v_cP) const { - _cP.resize(4, false); - - _cP[0] = cMo[0][0] * oP[0] + cMo[0][1] * oP[1] + cMo[0][2] * oP[2] + cMo[0][3] * oP[3]; - _cP[1] = cMo[1][0] * oP[0] + cMo[1][1] * oP[1] + cMo[1][2] * oP[2] + cMo[1][3] * oP[3]; - _cP[2] = cMo[2][0] * oP[0] + cMo[2][1] * oP[1] + cMo[2][2] * oP[2] + cMo[2][3] * oP[3]; - _cP[3] = cMo[3][0] * oP[0] + cMo[3][1] * oP[1] + cMo[3][2] * oP[2] + cMo[3][3] * oP[3]; - - double d = 1 / _cP[3]; - _cP[0] *= d; - _cP[1] *= d; - _cP[2] *= d; - _cP[3] *= d; + v_cP.resize(4, false); + + v_cP[0] = (cMo[0][0] * oP[0]) + (cMo[0][1] * oP[1]) + (cMo[0][2] * oP[2]) + (cMo[0][3] * oP[3]); + v_cP[1] = (cMo[1][0] * oP[0]) + (cMo[1][1] * oP[1]) + (cMo[1][2] * oP[2]) + (cMo[1][3] * oP[3]); + v_cP[2] = (cMo[2][0] * oP[0]) + (cMo[2][1] * oP[1]) + (cMo[2][2] * oP[2]) + (cMo[2][3] * oP[3]); + v_cP[3] = (cMo[3][0] * oP[0]) + (cMo[3][1] * oP[1]) + (cMo[3][2] * oP[2]) + (cMo[3][3] * oP[3]); + + double d = 1 / v_cP[3]; + v_cP[0] *= d; + v_cP[1] *= d; + v_cP[2] *= d; + v_cP[3] *= d; } /*! @@ -263,10 +265,10 @@ void vpPoint::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &_cP) cons */ void vpPoint::changeFrame(const vpHomogeneousMatrix &cMo) { - double X = cMo[0][0] * oP[0] + cMo[0][1] * oP[1] + cMo[0][2] * oP[2] + cMo[0][3] * oP[3]; - double Y = cMo[1][0] * oP[0] + cMo[1][1] * oP[1] + cMo[1][2] * oP[2] + cMo[1][3] * oP[3]; - double Z = cMo[2][0] * oP[0] + cMo[2][1] * oP[1] + cMo[2][2] * oP[2] + cMo[2][3] * oP[3]; - double W = cMo[3][0] * oP[0] + cMo[3][1] * oP[1] + cMo[3][2] * oP[2] + cMo[3][3] * oP[3]; + double X = (cMo[0][0] * oP[0]) + (cMo[0][1] * oP[1]) + (cMo[0][2] * oP[2]) + (cMo[0][3] * oP[3]); + double Y = (cMo[1][0] * oP[0]) + (cMo[1][1] * oP[1]) + (cMo[1][2] * oP[2]) + (cMo[1][3] * oP[3]); + double Z = (cMo[2][0] * oP[0]) + (cMo[2][1] * oP[1]) + (cMo[2][2] * oP[2]) + (cMo[2][3] * oP[3]); + double W = (cMo[3][0] * oP[0]) + (cMo[3][1] * oP[1]) + (cMo[3][2] * oP[2]) + (cMo[3][3] * oP[3]); double d = 1 / W; cP[0] = X * d; @@ -372,14 +374,15 @@ void vpPoint::display(const vpImage &I, const vpHomogeneousMatrix const vpColor &color, unsigned int thickness) { - vpColVector _cP, _p; - changeFrame(cMo, _cP); + vpColVector v_cP, v_p; + changeFrame(cMo, v_cP); - if (_cP[2] < 0) // no display if point is behind the camera + if (v_cP[2] < 0) { // no display if point is behind the camera return; + } - vpPoint::projection(_cP, _p); - vpFeatureDisplay::displayPoint(_p[0], _p[1], cam, I, color, thickness); + vpPoint::projection(v_cP, v_p); + vpFeatureDisplay::displayPoint(v_p[0], v_p[1], cam, I, color, thickness); } /*! @@ -396,14 +399,15 @@ void vpPoint::display(const vpImage &I, const vpHomogeneousMatrix void vpPoint::display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &color, unsigned int thickness) { - vpColVector _cP, _p; - changeFrame(cMo, _cP); + vpColVector v_cP, v_p; + changeFrame(cMo, v_cP); - if (_cP[2] < 0) // no display if point is behind the camera + if (v_cP[2] < 0) { // no display if point is behind the camera return; + } - vpPoint::projection(_cP, _p); - vpFeatureDisplay::displayPoint(_p[0], _p[1], cam, I, color, thickness); + vpPoint::projection(v_cP, v_p); + vpFeatureDisplay::displayPoint(v_p[0], v_p[1], cam, I, color, thickness); } VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpPoint & /* vpp */) { return (os << "vpPoint"); } diff --git a/modules/core/src/tracking/forward-projection/vpSphere.cpp b/modules/core/src/tracking/forward-projection/vpSphere.cpp index d915fe0638..1f892dd383 100644 --- a/modules/core/src/tracking/forward-projection/vpSphere.cpp +++ b/modules/core/src/tracking/forward-projection/vpSphere.cpp @@ -146,23 +146,23 @@ void vpSphere::projection(const vpColVector &cP_, vpColVector &p_) const y0 = cP_[1]; z0 = cP_[2]; - s = r * r - y0 * y0 - z0 * z0; + s = (r * r) - (y0 * y0) - (z0 * z0); - if ((s = z0 * z0 - r * r) < 0.0) { + if ((s = ((z0 * z0) - (r * r))) < 0.0) { vpERROR_TRACE("Error: Sphere is behind image plane\n"); } - p_[0] = x0 * z0 / s; // x - p_[1] = y0 * z0 / s; // y + p_[0] = (x0 * z0) / s; // x + p_[1] = (y0 * z0) / s; // y if (fabs(x0) > 1e-6) { double e = y0 / x0; double b = r / sqrt(s); - double a = x0 * x0 + y0 * y0 + z0 * z0 - r * r; + double a = ((x0 * x0) + (y0 * y0) + (z0 * z0)) - (r * r); if (a < 0.0) { vpERROR_TRACE("Error: Sphere is behind image plane\n"); } - a = r * sqrt(a) / s; + a = (r * sqrt(a)) / s; if (fabs(e) <= 1.0) { E = e; A = a; @@ -177,13 +177,13 @@ void vpSphere::projection(const vpColVector &cP_, vpColVector &p_) const else { E = 0.0; A = r / sqrt(s); - B = r * sqrt(y0 * y0 + z0 * z0 - r * r) / s; + B = (r * sqrt(((y0 * y0) + (z0 * z0)) - (r * r))) / s; } // Chaumette PhD Thesis 1990, eq 2.72 divided by 4 since n_ij = mu_ij_chaumette_thesis / 4 double det = 4 * (1.0 + vpMath::sqr(E)); double n20 = (vpMath::sqr(A) + vpMath::sqr(B * E)) / det; - double n11 = (vpMath::sqr(A) - vpMath::sqr(B)) * E / det; + double n11 = ((vpMath::sqr(A) - vpMath::sqr(B)) * E) / det; double n02 = (vpMath::sqr(B) + vpMath::sqr(A * E)) / det; p_[2] = n20; @@ -212,9 +212,9 @@ void vpSphere::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_) con double x0, y0, z0; // variables intermediaires - x0 = cMo[0][0] * oP[0] + cMo[0][1] * oP[1] + cMo[0][2] * oP[2] + cMo[0][3]; - y0 = cMo[1][0] * oP[0] + cMo[1][1] * oP[1] + cMo[1][2] * oP[2] + cMo[1][3]; - z0 = cMo[2][0] * oP[0] + cMo[2][1] * oP[1] + cMo[2][2] * oP[2] + cMo[2][3]; + x0 = (cMo[0][0] * oP[0]) + (cMo[0][1] * oP[1]) + (cMo[0][2] * oP[2]) + cMo[0][3]; + y0 = (cMo[1][0] * oP[0]) + (cMo[1][1] * oP[1]) + (cMo[1][2] * oP[2]) + cMo[1][3]; + z0 = (cMo[2][0] * oP[0]) + (cMo[2][1] * oP[1]) + (cMo[2][2] * oP[2]) + cMo[2][3]; cP_[3] = oP[3]; @@ -244,10 +244,10 @@ vpSphere *vpSphere::duplicate() const void vpSphere::display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &color, unsigned int thickness) { - vpColVector _cP, _p; - changeFrame(cMo, _cP); - projection(_cP, _p); - vpFeatureDisplay::displayEllipse(_p[0], _p[1], _p[2], _p[3], _p[4], cam, I, color, thickness); + vpColVector v_cP, v_p; + changeFrame(cMo, v_cP); + projection(v_cP, v_p); + vpFeatureDisplay::displayEllipse(v_p[0], v_p[1], v_p[2], v_p[3], v_p[4], cam, I, color, thickness); } /*! @@ -264,10 +264,10 @@ void vpSphere::display(const vpImage &I, const vpHomogeneousMatri void vpSphere::display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &color, unsigned int thickness) { - vpColVector _cP, _p; - changeFrame(cMo, _cP); - projection(_cP, _p); - vpFeatureDisplay::displayEllipse(_p[0], _p[1], _p[2], _p[3], _p[4], cam, I, color, thickness); + vpColVector v_cP, v_p; + changeFrame(cMo, v_cP); + projection(v_cP, v_p); + vpFeatureDisplay::displayEllipse(v_p[0], v_p[1], v_p[2], v_p[3], v_p[4], cam, I, color, thickness); } /*! diff --git a/modules/detection/include/visp3/detection/vpDetectorAprilTag.h b/modules/detection/include/visp3/detection/vpDetectorAprilTag.h index 3967392a98..7169c5007b 100644 --- a/modules/detection/include/visp3/detection/vpDetectorAprilTag.h +++ b/modules/detection/include/visp3/detection/vpDetectorAprilTag.h @@ -85,10 +85,10 @@ * * bool status = detector.detect(I); * if (status) { - * for(size_t i=0; i < detector.getNbObjects(); i++) { + * for(size_t i=0; i < detector.getNbObjects(); ++i) { * std::cout << "Tag code " << i << ":" << std::endl; * std::vector p = detector.getPolygon(i); - * for(size_t j=0; j < p.size(); j++) + * for(size_t j=0; j < p.size(); ++j) * std::cout << " Point " << j << ": " << p[j] << std::endl; * std::cout << " Message: \"" << detector.getMessage(i) << "\"" << std::endl; * } @@ -133,7 +133,7 @@ * * bool status = detector.detect(I, tagSize, cam, cMo); * if (status) { - * for(size_t i=0; i < detector.getNbObjects(); i++) { + * for(size_t i=0; i < detector.getNbObjects(); ++i) { * std::cout << "Tag number " << i << ":" << std::endl; * std::cout << " Message: \"" << detector.getMessage(i) << "\"" << std::endl; * std::cout << " Pose: " << vpPoseVector(cMo[i]).t() << std::endl; @@ -181,7 +181,7 @@ * * bool status = detector.detect(I); * if (status) { - * for(size_t i=0; i < detector.getNbObjects(); i++) { + * for(size_t i=0; i < detector.getNbObjects(); ++i) { * std::cout << "Tag code " << i << ":" << std::endl; * std::cout << " Message: \"" << detector.getMessage(i) << "\"" << std::endl; * if (detector.getMessage(i) == std::string("36h11 id: 0")) { diff --git a/modules/detection/src/tag/vpDetectorAprilTag.cpp b/modules/detection/src/tag/vpDetectorAprilTag.cpp index 0b435a8449..4ad66fe131 100644 --- a/modules/detection/src/tag/vpDetectorAprilTag.cpp +++ b/modules/detection/src/tag/vpDetectorAprilTag.cpp @@ -326,12 +326,12 @@ 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++) { + for (int i = 0; i < zarray_size(m_detections); ++i) { apriltag_detection_t *det; zarray_get(m_detections, i, &det); std::vector polygon; - for (int j = 0; j < 4; j++) { + for (int j = 0; j < 4; ++j) { polygon.push_back(vpImagePoint(det->p[j][1], det->p[j][0])); } polygons[static_cast(i)] = polygon; @@ -610,7 +610,7 @@ class vpDetectorAprilTag::Impl matd_t *p[4] = { matd_create_data(3, 1, data_p0), matd_create_data(3, 1, data_p1), matd_create_data(3, 1, data_p2), matd_create_data(3, 1, data_p3) }; matd_t *v[4]; - for (int i = 0; i < 4; i++) { + for (int i = 0; i < 4; ++i) { double data_v[] = { (det->p[i][0] - cam.get_u0()) / cam.get_px(), (det->p[i][1] - cam.get_v0()) / cam.get_py(), 1 }; v[i] = matd_create_data(3, 1, data_v); @@ -624,7 +624,7 @@ class vpDetectorAprilTag::Impl double err2; get_second_solution(v, p, &solution1, &solution2, nIters, &err2); - for (int i = 0; i < 4; i++) { + for (int i = 0; i < 4; ++i) { matd_destroy(p[i]); matd_destroy(v[i]); } diff --git a/modules/gui/include/visp3/gui/vpDisplayGDI.h b/modules/gui/include/visp3/gui/vpDisplayGDI.h index 597b893753..83477dd799 100644 --- a/modules/gui/include/visp3/gui/vpDisplayGDI.h +++ b/modules/gui/include/visp3/gui/vpDisplayGDI.h @@ -103,7 +103,7 @@ * std::cout << "Check keyboard events..." << std::endl; * char key[10]; * bool ret; - * for (int i=0; i< 200; i++) { + * for (int i=0; i< 200; ++i) { * bool ret = vpDisplay::getKeyboardEvent(I, key, false); * if (ret) * std::cout << "keyboard event: key: " << "\"" << key << "\"" << std::endl; diff --git a/modules/gui/include/visp3/gui/vpDisplayGTK.h b/modules/gui/include/visp3/gui/vpDisplayGTK.h index 9813f6a01c..ea4b093bc7 100644 --- a/modules/gui/include/visp3/gui/vpDisplayGTK.h +++ b/modules/gui/include/visp3/gui/vpDisplayGTK.h @@ -103,7 +103,7 @@ * std::cout << "Check keyboard events..." << std::endl; * char key[10]; * bool ret; - * for (int i=0; i< 200; i++) { + * for (int i=0; i< 200; ++i) { * bool ret = vpDisplay::getKeyboardEvent(I, key, false); * if (ret) * std::cout << "keyboard event: key: " << "\"" << key << "\"" << std::endl; diff --git a/modules/gui/include/visp3/gui/vpDisplayOpenCV.h b/modules/gui/include/visp3/gui/vpDisplayOpenCV.h index dc1b858583..30d552c592 100644 --- a/modules/gui/include/visp3/gui/vpDisplayOpenCV.h +++ b/modules/gui/include/visp3/gui/vpDisplayOpenCV.h @@ -111,7 +111,7 @@ * std::cout << "Check keyboard events..." << std::endl; * char key[10]; * bool ret; - * for (int i=0; i< 200; i++) { + * for (int i=0; i< 200; ++i) { * bool ret = vpDisplay::getKeyboardEvent(I, key, false); * if (ret) * std::cout << "keyboard event: key: " << "\"" << key << "\"" << std::endl; diff --git a/modules/gui/include/visp3/gui/vpDisplayX.h b/modules/gui/include/visp3/gui/vpDisplayX.h index cc843b4e3f..b7582422da 100644 --- a/modules/gui/include/visp3/gui/vpDisplayX.h +++ b/modules/gui/include/visp3/gui/vpDisplayX.h @@ -104,7 +104,7 @@ * std::cout << "Check keyboard events..." << std::endl; * char key[10]; * bool ret; - * for (int i=0; i< 200; i++) { + * for (int i=0; i< 200; ++i) { * bool ret = vpDisplay::getKeyboardEvent(I, key, false); * if (ret) * std::cout << "keyboard event: key: " << "\"" << key << "\"" << std::endl; diff --git a/modules/gui/include/visp3/gui/vpPlot.h b/modules/gui/include/visp3/gui/vpPlot.h index 2943e37869..1124417aeb 100644 --- a/modules/gui/include/visp3/gui/vpPlot.h +++ b/modules/gui/include/visp3/gui/vpPlot.h @@ -89,7 +89,7 @@ * // Add the point (0,2) to the second curve of the second graphic * A.plot(1,1,0,2); * - * for (int i = 0; i < 50; i++) { + * for (int i = 0; i < 50; ++i) { * // Add the point (i,sin(i*pi/10) in the first graphic * A.plot(0,0,i,sin(i*M_PI/10)); * diff --git a/modules/gui/src/display/vpDisplayGTK.cpp b/modules/gui/src/display/vpDisplayGTK.cpp index 6bb25f78f5..b43c2583c7 100644 --- a/modules/gui/src/display/vpDisplayGTK.cpp +++ b/modules/gui/src/display/vpDisplayGTK.cpp @@ -452,8 +452,8 @@ class vpDisplayGTK::Impl guchar OctetRouge, OctetVert, OctetBleu, mask; mask = 0x000000FF; - for (gint y = 0; y < height; y++) { - for (gint x = 0; x < width; x++) { + for (gint y = 0; y < height; ++y) { + for (gint x = 0; x < width; ++x) { pixel = gdk_image_get_pixel(ImageGtk, x, y); OctetBleu = static_cast(pixel) & mask; OctetVert = static_cast(pixel >> 8) & mask; diff --git a/modules/gui/src/display/windows/vpD3DRenderer.cpp b/modules/gui/src/display/windows/vpD3DRenderer.cpp index caae08fa6e..0ab06eb0e1 100644 --- a/modules/gui/src/display/windows/vpD3DRenderer.cpp +++ b/modules/gui/src/display/windows/vpD3DRenderer.cpp @@ -287,9 +287,9 @@ void vpD3DRenderer::initView(float WindowWidth, float WindowHeight) void vpD3DRenderer::convert(const vpImage &I, unsigned char *imBuffer, unsigned int pitch) { if (m_rscale == 1) { - for (unsigned int i = 0; i < m_rheight; i++) { + for (unsigned int i = 0; i < m_rheight; ++i) { unsigned int ii_ = i * pitch; - for (unsigned int j = 0; j < m_rwidth; j++) { + for (unsigned int j = 0; j < m_rwidth; ++j) { vpRGBa val = I[i][j]; unsigned int index_ = ii_ + j * 4; imBuffer[index_] = val.B; @@ -300,10 +300,10 @@ void vpD3DRenderer::convert(const vpImage &I, unsigned char *imBuffer, u } } else { - for (unsigned int i = 0; i < m_rheight; i++) { + for (unsigned int i = 0; i < m_rheight; ++i) { unsigned int i_ = i * m_rscale; unsigned int ii_ = i * pitch; - for (unsigned int j = 0; j < m_rwidth; j++) { + for (unsigned int j = 0; j < m_rwidth; ++j) { vpRGBa val = I[i_][j * m_rscale]; unsigned int index_ = ii_ + j * 4; imBuffer[index_] = val.B; @@ -324,9 +324,9 @@ void vpD3DRenderer::convert(const vpImage &I, unsigned char *imBuffer, u void vpD3DRenderer::convert(const vpImage &I, unsigned char *imBuffer, unsigned int pitch) { if (m_rscale == 1) { - for (unsigned int i = 0; i < m_rheight; i++) { + for (unsigned int i = 0; i < m_rheight; ++i) { unsigned int ii_ = i * pitch; - for (unsigned int j = 0; j < m_rwidth; j++) { + for (unsigned int j = 0; j < m_rwidth; ++j) { unsigned char val = I[i][j]; unsigned int index_ = ii_ + j * 4; imBuffer[index_] = val; @@ -337,10 +337,10 @@ void vpD3DRenderer::convert(const vpImage &I, unsigned char *imBu } } else { - for (unsigned int i = 0; i < m_rheight; i++) { + for (unsigned int i = 0; i < m_rheight; ++i) { unsigned int i_ = i * m_rscale; unsigned int ii_ = i * pitch; - for (unsigned int j = 0; j < m_rwidth; j++) { + for (unsigned int j = 0; j < m_rwidth; ++j) { unsigned char val = I[i_][j * m_rscale]; unsigned int index_ = ii_ + j * 4; imBuffer[index_] = val; @@ -367,10 +367,10 @@ void vpD3DRenderer::convertROI(const vpImage &I, unsigned char *i int w = j_max - j_min; if (m_rscale == 1) { - for (int i = 0; i < h; i++) { + for (int i = 0; i < h; ++i) { unsigned int i_ = i_min + i; unsigned int ii_ = i * pitch; - for (int j = 0; j < w; j++) { + for (int j = 0; j < w; ++j) { unsigned char val = I[i_][j_min + j]; unsigned int index_ = ii_ + j * 4; imBuffer[index_] = val; @@ -381,10 +381,10 @@ void vpD3DRenderer::convertROI(const vpImage &I, unsigned char *i } } else { - for (int i = 0; i < h; i++) { + for (int i = 0; i < h; ++i) { unsigned int i_ = (i_min + i) * m_rscale; unsigned int ii_ = i * pitch; - for (int j = 0; j < w; j++) { + for (int j = 0; j < w; ++j) { unsigned char val = I[i_][(j_min + j) * m_rscale]; unsigned int index_ = ii_ + j * 4; imBuffer[index_] = val; @@ -411,10 +411,10 @@ void vpD3DRenderer::convertROI(const vpImage &I, unsigned char *imBuffer int w = j_max - j_min; if (m_rscale == 1) { - for (int i = 0; i < h; i++) { + for (int i = 0; i < h; ++i) { unsigned int i_ = i_min + i; unsigned int ii_ = i * pitch; - for (int j = 0; j < w; j++) { + for (int j = 0; j < w; ++j) { vpRGBa val = I[i_][j_min + j]; unsigned int index_ = ii_ + j * 4; imBuffer[index_] = val.B; @@ -425,10 +425,10 @@ void vpD3DRenderer::convertROI(const vpImage &I, unsigned char *imBuffer } } else { - for (int i = 0; i < h; i++) { + for (int i = 0; i < h; ++i) { unsigned int i_ = (i_min + i) * m_rscale; unsigned int ii_ = i * pitch; - for (int j = 0; j < w; j++) { + for (int j = 0; j < w; ++j) { vpRGBa val = I[i_][(j_min + j) * m_rscale]; unsigned int index_ = ii_ + j * 4; imBuffer[index_] = val.B; @@ -834,8 +834,8 @@ void vpD3DRenderer::drawRect(const vpImagePoint &topLeft, unsigned int width, un if (topLeftScaled.get_i() >= 0 && topLeftScaled.get_j() + widthScaled < m_rwidth && topLeftScaled.get_i() + heightScaled < m_rheight && topLeftScaled.get_j() >= 0) { - for (int x = 0; x < rectW; x++) { - for (int y = 0; y < rectH; y++) + for (int x = 0; x < rectW; ++x) { + for (int y = 0; y < rectH; ++y) setBufferPixel(buf, pitch, x, y, color); } } @@ -1133,7 +1133,7 @@ void TextureToRGBa(vpImage &I, unsigned char *imBuffer, unsigned int pit unsigned int j = I.getWidth(); unsigned int k = 0; - for (unsigned int i = 0; i < I.getHeight() * I.getWidth(); i++) { + for (unsigned int i = 0; i < I.getHeight() * I.getWidth(); ++i) { // go to the next line if (j == 0) { k += pitch - (I.getWidth() * 4); diff --git a/modules/gui/src/display/windows/vpGDIRenderer.cpp b/modules/gui/src/display/windows/vpGDIRenderer.cpp index 7a8de6d807..cea70d073f 100644 --- a/modules/gui/src/display/windows/vpGDIRenderer.cpp +++ b/modules/gui/src/display/windows/vpGDIRenderer.cpp @@ -218,7 +218,7 @@ void vpGDIRenderer::convert(const vpImage &I, HBITMAP &hBmp) unsigned char *imBuffer = new unsigned char[m_rwidth * m_rheight * 4]; if (m_rscale == 1) { - for (unsigned int i = 0, k = 0; i < m_rwidth * m_rheight * 4; i += 4, k++) { + for (unsigned int i = 0, k = 0; i < m_rwidth * m_rheight * 4; i += 4, ++k) { imBuffer[i + 0] = I.bitmap[k].B; imBuffer[i + 1] = I.bitmap[k].G; imBuffer[i + 2] = I.bitmap[k].R; @@ -226,10 +226,10 @@ void vpGDIRenderer::convert(const vpImage &I, HBITMAP &hBmp) } } else { - for (unsigned int i = 0; i < m_rheight; i++) { + for (unsigned int i = 0; i < m_rheight; ++i) { unsigned int i_ = i * m_rscale; unsigned int ii_ = i * m_rwidth; - for (unsigned int j = 0; j < m_rwidth; j++) { + for (unsigned int j = 0; j < m_rwidth; ++j) { vpRGBa val = I[i_][j * m_rscale]; unsigned int index_ = (ii_ + j) * 4; imBuffer[index_] = val.B; @@ -287,10 +287,10 @@ void vpGDIRenderer::convertROI(const vpImage &I, const vpImagePoint &iP, } } else { - for (int i = 0; i < h; i++) { + for (int i = 0; i < h; ++i) { unsigned int i_ = (i_min + i) * m_rscale; unsigned int ii_ = i * w; - for (int j = 0; j < w; j++) { + for (int j = 0; j < w; ++j) { vpRGBa val = I[i_][(j_min + j) * m_rscale]; unsigned int index_ = (ii_ + j) * 4; imBuffer[index_] = val.B; @@ -319,7 +319,7 @@ void vpGDIRenderer::convert(const vpImage &I, HBITMAP &hBmp) unsigned char *imBuffer = new unsigned char[m_rwidth * m_rheight * 4]; if (m_rscale == 1) { - for (unsigned int i = 0, k = 0; i < m_rwidth * m_rheight * 4; i += 4, k++) { + for (unsigned int i = 0, k = 0; i < m_rwidth * m_rheight * 4; i += 4, ++k) { imBuffer[i + 0] = I.bitmap[k]; imBuffer[i + 1] = I.bitmap[k]; imBuffer[i + 2] = I.bitmap[k]; @@ -327,10 +327,10 @@ void vpGDIRenderer::convert(const vpImage &I, HBITMAP &hBmp) } } else { - for (unsigned int i = 0; i < m_rheight; i++) { + for (unsigned int i = 0; i < m_rheight; ++i) { unsigned int i_ = i * m_rscale; unsigned int ii_ = i * m_rwidth; - for (unsigned int j = 0; j < m_rwidth; j++) { + for (unsigned int j = 0; j < m_rwidth; ++j) { unsigned char val = I[i_][j * m_rscale]; unsigned int index_ = (ii_ + j) * 4; imBuffer[index_] = val; @@ -369,10 +369,10 @@ void vpGDIRenderer::convertROI(const vpImage &I, const vpImagePoi unsigned char *imBuffer = new unsigned char[w * h * 4]; if (m_rscale == 1) { - for (int i = 0; i < h; i++) { + for (int i = 0; i < h; ++i) { unsigned int i_ = i_min + i; unsigned int ii_ = i * w; - for (int j = 0; j < w; j++) { + for (int j = 0; j < w; ++j) { unsigned char val = I[i_][j_min + j]; unsigned int index_ = (ii_ + j) * 4; imBuffer[index_] = val; @@ -383,10 +383,10 @@ void vpGDIRenderer::convertROI(const vpImage &I, const vpImagePoi } } else { - for (int i = 0; i < h; i++) { + for (int i = 0; i < h; ++i) { unsigned int i_ = (i_min + i) * m_rscale; unsigned int ii_ = i * w; - for (int j = 0; j < w; j++) { + for (int j = 0; j < w; ++j) { unsigned char val = I[i_][(j_min + j) * m_rscale]; unsigned int index_ = (ii_ + j) * 4; imBuffer[index_] = val; diff --git a/modules/gui/src/plot/vpPlot.cpp b/modules/gui/src/plot/vpPlot.cpp index f85f67f39d..da074c1a12 100644 --- a/modules/gui/src/plot/vpPlot.cpp +++ b/modules/gui/src/plot/vpPlot.cpp @@ -182,7 +182,7 @@ void vpPlot::initNbGraph(unsigned int nbGraph) break; } - for (unsigned int i = 0; i < graphNbr; i++) { + for (unsigned int i = 0; i < graphNbr; ++i) { graphList[i].title.clear(); graphList[i].unitx.clear(); graphList[i].unity.clear(); @@ -252,7 +252,7 @@ void vpPlot::setColor(unsigned int graphNum, unsigned int curveNum, vpColor colo */ void vpPlot::displayGrid() { - for (unsigned int i = 0; i < graphNbr; i++) + for (unsigned int i = 0; i < graphNbr; ++i) graphList[i].displayGrid(I); } @@ -450,7 +450,7 @@ void vpPlot::navigate() while (b != vpMouseButton::button3) { if (!blocked) { vpDisplay::getPointerPosition(I, iP); - for (unsigned int i = 0; i < graphNbr; i++) { + for (unsigned int i = 0; i < graphNbr; ++i) { if (iP.inRectangle((graphList + i)->graphZone)) { iblocked = i; break; @@ -486,7 +486,7 @@ void vpPlot::getPixelValue(bool block) else vpDisplay::getPointerPosition(I, iP); - for (unsigned int i = 0; i < graphNbr; i++) { + for (unsigned int i = 0; i < graphNbr; ++i) { if ((graphList + i)->getPixelValue(I, iP)) break; } @@ -557,7 +557,7 @@ void vpPlot::setLegend(unsigned int graphNum, unsigned int curveNum, const std:: */ void vpPlot::resetPointList(unsigned int graphNum) { - for (unsigned int i = 0; i < (graphList + graphNum)->curveNbr; i++) + for (unsigned int i = 0; i < (graphList + graphNum)->curveNbr; ++i) (graphList + graphNum)->resetPointList(i); } @@ -585,7 +585,7 @@ void vpPlot::setThickness(unsigned int graphNum, unsigned int curveNum, unsigned */ void vpPlot::setGraphThickness(unsigned int graphNum, unsigned int thickness) { - for (unsigned int curveNum = 0; curveNum < (graphList + graphNum)->curveNbr; curveNum++) + for (unsigned int curveNum = 0; curveNum < (graphList + graphNum)->curveNbr; ++curveNum) (graphList + graphNum)->setCurveThickness(curveNum, thickness); } @@ -660,7 +660,7 @@ void vpPlot::saveData(unsigned int graphNum, const std::string &dataFile, const fichier << title_prefix << (graphList + graphNum)->title << std::endl; - for (ind = 0; ind < (graphList + graphNum)->curveNbr; ind++) { + for (ind = 0; ind < (graphList + graphNum)->curveNbr; ++ind) { vec_iter_pointListx[ind] = (graphList + graphNum)->curveList[ind].pointListx.begin(); vec_iter_pointListy[ind] = (graphList + graphNum)->curveList[ind].pointListy.begin(); vec_iter_pointListz[ind] = (graphList + graphNum)->curveList[ind].pointListz.begin(); @@ -671,7 +671,7 @@ void vpPlot::saveData(unsigned int graphNum, const std::string &dataFile, const while (end == false) { end = true; - for (ind = 0; ind < (graphList + graphNum)->curveNbr; ind++) { + for (ind = 0; ind < (graphList + graphNum)->curveNbr; ++ind) { // if (!(graphList+graphNum)->curveList[ind].pointListx.outside() // && // !(graphList+graphNum)->curveList[ind].pointListy.outside() diff --git a/modules/gui/src/plot/vpPlotGraph.cpp b/modules/gui/src/plot/vpPlotGraph.cpp index d27f45d54a..6d772a7d1a 100644 --- a/modules/gui/src/plot/vpPlotGraph.cpp +++ b/modules/gui/src/plot/vpPlotGraph.cpp @@ -89,7 +89,7 @@ void vpPlotGraph::initGraph(unsigned int nbCurve) vpColor colors[6] = { vpColor::blue, vpColor::green, vpColor::red, vpColor::cyan, vpColor::orange, vpColor::purple }; - for (unsigned int i = 0; i < curveNbr; i++) { + for (unsigned int i = 0; i < curveNbr; ++i) { (curveList + i)->color = colors[i % 6]; (curveList + i)->curveStyle = vpPlotCurve::line; (curveList + i)->pointListx.clear(); @@ -173,7 +173,7 @@ void vpPlotGraph::findPose() // Instead of pose computation we use an approximation double Z = 0; - for (unsigned int i = 0; i < 4; i++) { + for (unsigned int i = 0; i < 4; ++i) { vpPixelMeterConversion::convertPoint(cam, iP[i], x, y); Z = vpMath::maximum(Z, point_[i].get_oX() / x); Z = vpMath::maximum(Z, point_[i].get_oY() / y); @@ -384,14 +384,14 @@ void vpPlotGraph::displayTitle(vpImage &I) void vpPlotGraph::displayLegend(vpImage &I) { size_t offsetj = 0; - for (unsigned int i = 0; i < curveNbr; i++) { + for (unsigned int i = 0; i < curveNbr; ++i) { size_t offset = epsj * (curveList + i)->legend.size(); offsetj = vpMath::maximum(offset, offsetj); } if (offsetj > dWidth) offsetj = dWidth; - for (unsigned int i = 0; i < curveNbr; i++) { + for (unsigned int i = 0; i < curveNbr; ++i) { vpDisplay::displayText(I, vpImagePoint(dTopLeft.get_i() + i * 5 * epsi, dTopLeft.get_j() + dWidth - offsetj), (curveList + i)->legend.c_str(), (curveList + i)->color); } @@ -536,7 +536,7 @@ void vpPlotGraph::replot(vpImage &I) { clearGraphZone(I); displayGrid(I); - for (unsigned int i = 0; i < curveNbr; i++) + for (unsigned int i = 0; i < curveNbr; ++i) (curveList + i)->plotList(I, xorg, yorg, zoomx, zoomy); vpDisplay::flushROI(I, graphZone); } @@ -870,7 +870,7 @@ void vpPlotGraph::displayGrid3D(vpImage &I) pt[5].setWorldCoordinates(ptXorg, ptYorg, w_zval); vpImagePoint iP[6]; - for (unsigned int i = 0; i < 6; i++) { + for (unsigned int i = 0; i < 6; ++i) { pt[i].track(cMo); double u = 0.0, v = 0.0; vpMeterPixelConversion::convertPoint(cam, pt[i].get_x(), pt[i].get_y(), u, v); @@ -1192,7 +1192,7 @@ void vpPlotGraph::replot3D(vpImage &I) clearGraphZone(I); displayGrid3D(I); - for (unsigned int i = 0; i < curveNbr; i++) { + for (unsigned int i = 0; i < curveNbr; ++i) { std::list::const_iterator it_ptListx = (curveList + i)->pointListx.begin(); std::list::const_iterator it_ptListy = (curveList + i)->pointListy.begin(); std::list::const_iterator it_ptListz = (curveList + i)->pointListz.begin(); diff --git a/modules/imgproc/src/vpCLAHE.cpp b/modules/imgproc/src/vpCLAHE.cpp index 3d97d5a848..b4469d5bf4 100644 --- a/modules/imgproc/src/vpCLAHE.cpp +++ b/modules/imgproc/src/vpCLAHE.cpp @@ -81,18 +81,18 @@ namespace { -int fastRound(float value) { return (int)(value + 0.5f); } +int fastRound(float value) { return static_cast(value + 0.5f); } void clipHistogram(const std::vector &hist, std::vector &clippedHist, int limit) { clippedHist = hist; int clippedEntries = 0, clippedEntriesBefore = 0; - int histlength = (int)hist.size(); + int histlength = static_cast(hist.size()); do { clippedEntriesBefore = clippedEntries; clippedEntries = 0; - for (int i = 0; i < histlength; i++) { + for (int i = 0; i < histlength; ++i) { int d = clippedHist[i] - limit; if (d > 0) { clippedEntries += d; @@ -100,9 +100,9 @@ void clipHistogram(const std::vector &hist, std::vector &clippedHist, } } - int d = clippedEntries / (histlength); - int m = clippedEntries % (histlength); - for (int i = 0; i < histlength; i++) { + int d = clippedEntries / histlength; + int m = clippedEntries % histlength; + for (int i = 0; i < histlength; ++i) { clippedHist[i] += d; } @@ -122,8 +122,8 @@ void createHistogram(int blockRadius, int bins, int blockXCenter, int blockYCent int xMin = std::max(0, blockXCenter - blockRadius); int yMin = std::max(0, blockYCenter - blockRadius); - int xMax = std::min((int)I.getWidth(), blockXCenter + blockRadius + 1); - int yMax = std::min((int)I.getHeight(), blockYCenter + blockRadius + 1); + int xMax = std::min(static_cast(I.getWidth()), blockXCenter + blockRadius + 1); + int yMax = std::min(static_cast(I.getHeight()), blockYCenter + blockRadius + 1); for (int y = yMin; y < yMax; ++y) { for (int x = xMin; x < xMax; ++x) { @@ -135,7 +135,7 @@ void createHistogram(int blockRadius, int bins, int blockXCenter, int blockYCent std::vector createTransfer(const std::vector &hist, int limit, std::vector &cdfs) { clipHistogram(hist, cdfs, limit); - int hMin = (int)hist.size() - 1; + int hMin = static_cast(hist.size()) - 1; for (int i = 0; i < hMin; ++i) { if (cdfs[i] != 0) { @@ -143,7 +143,8 @@ std::vector createTransfer(const std::vector &hist, int limit, std:: } } int cdf = 0; - for (int i = hMin; i < (int)hist.size(); ++i) { + int hist_size = static_cast(hist.size()); + for (int i = hMin; i < hist_size; ++i) { cdf += cdfs[i]; cdfs[i] = cdf; } @@ -152,8 +153,9 @@ std::vector createTransfer(const std::vector &hist, int limit, std:: int cdfMax = cdfs[hist.size() - 1]; std::vector transfer(hist.size()); - for (int i = 0; i < (int)transfer.size(); ++i) { - transfer[i] = (cdfs[i] - cdfMin) / (float)(cdfMax - cdfMin); + int transfer_size = static_cast(transfer.size()); + for (int i = 0; i < transfer_size; ++i) { + transfer[i] = (cdfs[i] - cdfMin) / static_cast(cdfMax - cdfMin); } return transfer; @@ -161,16 +163,16 @@ std::vector createTransfer(const std::vector &hist, int limit, std:: float transferValue(int v, std::vector &clippedHist) { - int clippedHistLength = (int)clippedHist.size(); + int clippedHistLength = static_cast(clippedHist.size()); int hMin = clippedHistLength - 1; - for (int i = 0; i < hMin; i++) { + for (int i = 0; i < hMin; ++i) { if (clippedHist[i] != 0) { hMin = i; } } int cdf = 0; - for (int i = hMin; i <= v; i++) { + for (int i = hMin; i <= v; ++i) { cdf += clippedHist[i]; } @@ -180,7 +182,7 @@ float transferValue(int v, std::vector &clippedHist) } int cdfMin = clippedHist[hMin]; - return (cdf - cdfMin) / (float)(cdfMax - cdfMin); + return (cdf - cdfMin) / static_cast(cdfMax - cdfMin); } float transferValue(int v, const std::vector &hist, std::vector &clippedHist, int limit) @@ -200,12 +202,12 @@ void clahe(const vpImage &I1, vpImage &I2, int blo return; } - if (bins < 0 || bins > 256) { + if ((bins < 0) || (bins > 256)) { std::cerr << "Error: (bins < 0 || bins > 256)!" << std::endl; return; } - if ((unsigned int)(2 * blockRadius + 1) > I1.getWidth() || (unsigned int)(2 * blockRadius + 1) > I1.getHeight()) { + if ((static_cast((2 * blockRadius) + 1) > I1.getWidth()) || (static_cast((2 * blockRadius) + 1) > I1.getHeight())) { std::cerr << "Error: (unsigned int) (2*blockRadius+1) > I1.getWidth() || " "(unsigned int) (2*blockRadius+1) > I1.getHeight()!" << std::endl; @@ -215,29 +217,29 @@ void clahe(const vpImage &I1, vpImage &I2, int blo I2.resize(I1.getHeight(), I1.getWidth()); if (fast) { - int blockSize = 2 * blockRadius + 1; - int limit = (int)(slope * blockSize * blockSize / bins + 0.5); + int blockSize = (2 * blockRadius) + 1; + int limit = static_cast((slope * blockSize * blockSize / bins) + 0.5); /* div */ int nc = I1.getWidth() / blockSize; int nr = I1.getHeight() / blockSize; /* % */ - int cm = I1.getWidth() - nc * blockSize; + int cm = I1.getWidth() - (nc * blockSize); std::vector cs; switch (cm) { case 0: cs.resize(nc); for (int i = 0; i < nc; ++i) { - cs[i] = i * blockSize + blockRadius + 1; + cs[i] = (i * blockSize) + blockRadius + 1; } break; case 1: cs.resize(nc + 1); for (int i = 0; i < nc; ++i) { - cs[i] = i * blockSize + blockRadius + 1; + cs[i] = (i * blockSize) + blockRadius + 1; } cs[nc] = I1.getWidth() - blockRadius - 1; break; @@ -246,49 +248,50 @@ void clahe(const vpImage &I1, vpImage &I2, int blo cs.resize(nc + 2); cs[0] = blockRadius + 1; for (int i = 0; i < nc; ++i) { - cs[i + 1] = i * blockSize + blockRadius + 1 + cm / 2; + cs[i + 1] = (i * blockSize) + blockRadius + 1 + (cm / 2); } cs[nc + 1] = I1.getWidth() - blockRadius - 1; } - int rm = I1.getHeight() - nr * blockSize; + int rm = I1.getHeight() - (nr * blockSize); std::vector rs; switch (rm) { case 0: - rs.resize((size_t)nr); + rs.resize(static_cast(nr)); for (int i = 0; i < nr; ++i) { - rs[i] = i * blockSize + blockRadius + 1; + rs[i] = (i * blockSize) + blockRadius + 1; } break; case 1: - rs.resize((size_t)(nr + 1)); + rs.resize(static_cast(nr + 1)); for (int i = 0; i < nr; ++i) { - rs[i] = i * blockSize + blockRadius + 1; + rs[i] = (i * blockSize) + blockRadius + 1; } rs[nr] = I1.getHeight() - blockRadius - 1; break; default: - rs.resize((size_t)(nr + 2)); + rs.resize(static_cast(nr + 2)); rs[0] = blockRadius + 1; for (int i = 0; i < nr; ++i) { - rs[i + 1] = i * blockSize + blockRadius + 1 + rm / 2; + rs[i + 1] = (i * blockSize) + blockRadius + 1 + (rm / 2); } rs[nr + 1] = I1.getHeight() - blockRadius - 1; } - std::vector hist((size_t)(bins + 1)); - std::vector cdfs((size_t)(bins + 1)); + std::vector hist(static_cast(bins + 1)); + std::vector cdfs(static_cast(bins + 1)); std::vector tl; std::vector tr; std::vector br; std::vector bl; - for (int r = 0; r <= (int)rs.size(); ++r) { + int rs_size = static_cast(rs.size()); + for (int r = 0; r <= rs_size; ++r) { int r0 = std::max(0, r - 1); - int r1 = std::min((int)rs.size() - 1, r); + int r1 = std::min(static_cast(rs.size()) - 1, r); int dr = rs[r1] - rs[r0]; createHistogram(blockRadius, bins, cs[0], rs[r0], I1, hist); @@ -302,11 +305,12 @@ void clahe(const vpImage &I1, vpImage &I2, int blo } int yMin = (r == 0 ? 0 : rs[r0]); - int yMax = (r < (int)rs.size() ? rs[r1] : I1.getHeight()); + int yMax = (r < static_cast(rs.size()) ? rs[r1] : I1.getHeight()); - for (int c = 0; c <= (int)cs.size(); ++c) { + int cs_size = static_cast(cs.size()); + for (int c = 0; c <= cs_size; ++c) { int c0 = std::max(0, c - 1); - int c1 = std::min((int)cs.size() - 1, c); + int c1 = std::min(static_cast(cs.size()) - 1, c); int dc = cs[c1] - cs[c0]; tl = tr; @@ -325,12 +329,12 @@ void clahe(const vpImage &I1, vpImage &I2, int blo } int xMin = (c == 0 ? 0 : cs[c0]); - int xMax = (c < (int)cs.size() ? cs[c1] : I1.getWidth()); + int xMax = (c < static_cast(cs.size()) ? cs[c1] : I1.getWidth()); for (int y = yMin; y < yMax; ++y) { - float wy = (float)(rs[r1] - y) / dr; + float wy = static_cast(rs[r1] - y) / dr; for (int x = xMin; x < xMax; ++x) { - float wx = (float)(cs[c1] - x) / dc; + float wx = static_cast(cs[c1] - x) / dc; int v = fastRound(I1[y][x] / 255.0f * bins); float t00 = tl[v]; float t01 = tr[v]; @@ -343,11 +347,11 @@ void clahe(const vpImage &I1, vpImage &I2, int blo t1 = t10; } else { - t0 = wx * t00 + (1.0f - wx) * t01; - t1 = wx * t10 + (1.0f - wx) * t11; + t0 = (wx * t00) + ((1.0f - wx) * t01); + t1 = (wx * t10) + ((1.0f - wx) * t11); } - float t = (r0 == r1) ? t0 : wy * t0 + (1.0f - wy) * t1; + float t = (r0 == r1) ? t0 : (wy * t0) + ((1.0f - wy) * t1); I2[y][x] = std::max(0, std::min(255, fastRound(t * 255.0f))); } } @@ -360,18 +364,19 @@ void clahe(const vpImage &I1, vpImage &I2, int blo bool first = true; int xMin0 = 0; - int xMax0 = std::min((int)I1.getWidth(), blockRadius); + int xMax0 = std::min(static_cast(I1.getWidth()), blockRadius); - for (int y = 0; y < (int)I1.getHeight(); y++) { - int yMin = std::max(0, y - (int)blockRadius); - int yMax = std::min((int)I1.getHeight(), y + blockRadius + 1); + int i1_height = static_cast(I1.getHeight()); + for (int y = 0; y < i1_height; ++y) { + int yMin = std::max(0, y - static_cast(blockRadius)); + int yMax = std::min(static_cast(I1.getHeight()), y + blockRadius + 1); int h = yMax - yMin; #if 0 std::fill(hist.begin(), hist.end(), 0); // Compute histogram for the current block - for (int yi = yMin; yi < yMax; yi++) { - for (int xi = xMin0; xi < xMax0; xi++) { + for (int yi = yMin; yi < yMax; ++yi) { + for (int xi = xMin0; xi < xMax0; ++xi) { ++hist[fastRound(I1[yi][xi] / 255.0f * bins)]; } } @@ -379,8 +384,8 @@ void clahe(const vpImage &I1, vpImage &I2, int blo if (first) { first = false; // Compute histogram for the block at (0,0) - for (int yi = yMin; yi < yMax; yi++) { - for (int xi = xMin0; xi < xMax0; xi++) { + for (int yi = yMin; yi < yMax; ++yi) { + for (int xi = xMin0; xi < xMax0; ++xi) { ++hist[fastRound(I1[yi][xi] / 255.0f * bins)]; } } @@ -391,46 +396,46 @@ void clahe(const vpImage &I1, vpImage &I2, int blo if (yMin > 0) { int yMin1 = yMin - 1; // Sliding histogram, remove top - for (int xi = xMin0; xi < xMax0; xi++) { + for (int xi = xMin0; xi < xMax0; ++xi) { --hist[fastRound(I1[yMin1][xi] / 255.0f * bins)]; } } - if (y + blockRadius < (int)I1.getHeight()) { + if ((y + blockRadius) < static_cast(I1.getHeight())) { int yMax1 = yMax - 1; // Sliding histogram, add bottom - for (int xi = xMin0; xi < xMax0; xi++) { + for (int xi = xMin0; xi < xMax0; ++xi) { ++hist[fastRound(I1[yMax1][xi] / 255.0f * bins)]; } } } prev_hist = hist; #endif - - for (int x = 0; x < (int)I1.getWidth(); x++) { - int xMin = std::max(0, x - (int)blockRadius); + int i1_width = static_cast(I1.getWidth()); + for (int x = 0; x < i1_width; ++x) { + int xMin = std::max(0, x - static_cast(blockRadius)); int xMax = x + blockRadius + 1; if (xMin > 0) { int xMin1 = xMin - 1; // Sliding histogram, remove left - for (int yi = yMin; yi < yMax; yi++) { + for (int yi = yMin; yi < yMax; ++yi) { --hist[fastRound(I1[yi][xMin1] / 255.0f * bins)]; } } - if (xMax <= (int)I1.getWidth()) { + if (xMax <= static_cast(I1.getWidth())) { int xMax1 = xMax - 1; // Sliding histogram, add right - for (int yi = yMin; yi < yMax; yi++) { + for (int yi = yMin; yi < yMax; ++yi) { ++hist[fastRound(I1[yi][xMax1] / 255.0f * bins)]; } } int v = fastRound(I1[y][x] / 255.0f * bins); - int w = std::min((int)I1.getWidth(), xMax) - xMin; + int w = std::min(static_cast(I1.getWidth()), xMax) - xMin; int n = h * w; - int limit = (int)(slope * n / bins + 0.5f); + int limit = static_cast(slope * n / bins + 0.5f); I2[y][x] = fastRound(transferValue(v, hist, clippedHist, limit) * 255.0f); } } @@ -455,8 +460,8 @@ void clahe(const vpImage &I1, vpImage &I2, int blockRadius, int I2.resize(I1.getHeight(), I1.getWidth()); unsigned int size = I2.getWidth() * I2.getHeight(); - unsigned char *ptrStart = (unsigned char *)I2.bitmap; - unsigned char *ptrEnd = ptrStart + size * 4; + unsigned char *ptrStart = (unsigned char *)(I2.bitmap); + unsigned char *ptrEnd = ptrStart + (size * 4); unsigned char *ptrCurrent = ptrStart; unsigned int cpt = 0; diff --git a/modules/imgproc/src/vpCircleHoughTransform.cpp b/modules/imgproc/src/vpCircleHoughTransform.cpp index ef6d1107c3..39b0a6b051 100644 --- a/modules/imgproc/src/vpCircleHoughTransform.cpp +++ b/modules/imgproc/src/vpCircleHoughTransform.cpp @@ -185,7 +185,8 @@ vpCircleHoughTransform::initGradientFilters() for (unsigned int c = 0; c < nbCols; ++c) { filter[r][c] = filter[r][c] * scale; } - }}; + } + }; #endif if ((m_algoParams.m_gradientFilterKernelSize % 2) != 1) { @@ -255,7 +256,7 @@ vpCircleHoughTransform::detect(const vpImage &I, const int &nbCir auto hasBetterProba = [](std::pair a, std::pair b) { return (a.second > b.second); - }; + }; #endif std::sort(v_id_proba.begin(), v_id_proba.end(), hasBetterProba); @@ -697,19 +698,19 @@ vpCircleHoughTransform::computeCenterCandidates() const int &offsetX, const int &offsetY, const int &nbCols, const int &nbRows, vpImage &accum, bool &hasToStop) { - if (((x - offsetX) < 0) || - ((x - offsetX) >= nbCols) || - ((y - offsetY) < 0) || - ((y - offsetY) >= nbRows) - ) { - hasToStop = true; - } - else { - float dx = (x_orig - static_cast(x)); - float dy = (y_orig - static_cast(y)); - accum[y - offsetY][x - offsetX] += std::abs(dx) + std::abs(dy); - } - }; + if (((x - offsetX) < 0) || + ((x - offsetX) >= nbCols) || + ((y - offsetY) < 0) || + ((y - offsetY) >= nbRows) + ) { + hasToStop = true; + } + else { + float dx = (x_orig - static_cast(x)); + float dy = (y_orig - static_cast(y)); + accum[y - offsetY][x - offsetX] += std::abs(dx) + std::abs(dy); + } + }; #endif updateAccumulator(x1, y1, x_low, y_low, @@ -746,9 +747,9 @@ vpCircleHoughTransform::computeCenterCandidates() int nbVotes = -1; std::vector, float> > peak_positions_votes; - for (int y = 0; y < nbRowsAccum; y++) { + for (int y = 0; y < nbRowsAccum; ++y) { int left = -1; - for (int x = 0; x < nbColsAccum; x++) { + for (int x = 0; x < nbColsAccum; ++x) { if ((centersAccum[y][x] >= m_algoParams.m_centerMinThresh) && (vpMath::equal(centersAccum[y][x], centerCandidatesMaxima[y][x])) && (centersAccum[y][x] > centersAccum[y][x + 1]) @@ -828,8 +829,8 @@ vpCircleHoughTransform::computeCenterCandidates() } // Computing the distance with the peak of insterest std::pair position_candidate = peak_positions_votes[idCandidate].first; - float squared_distance = (position.first - position_candidate.first) * (position.first - position_candidate.first) - + (position.second - position_candidate.second) * (position.second - position_candidate.second); + float squared_distance = ((position.first - position_candidate.first) * (position.first - position_candidate.first)) + + ((position.second - position_candidate.second) * (position.second - position_candidate.second)); // If the peaks are similar, update the barycenter peak between them and corresponding votes if (squared_distance < squared_distance_max) { @@ -854,8 +855,8 @@ vpCircleHoughTransform::computeCenterCandidates() #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) auto sortingCenters = [](const std::pair, float> &position_vote_a, const std::pair, float> &position_vote_b) { - return position_vote_a.second > position_vote_b.second; - }; + return position_vote_a.second > position_vote_b.second; + }; #endif std::sort(merged_peaks_position_votes.begin(), merged_peaks_position_votes.end(), sortingCenters); @@ -863,7 +864,7 @@ vpCircleHoughTransform::computeCenterCandidates() nbPeaks = static_cast(merged_peaks_position_votes.size()); int nbPeaksToKeep = (m_algoParams.m_expectedNbCenters > 0 ? m_algoParams.m_expectedNbCenters : static_cast(nbPeaks)); nbPeaksToKeep = std::min(nbPeaksToKeep, static_cast(nbPeaks)); - for (int i = 0; i < nbPeaksToKeep; i++) { + for (int i = 0; i < nbPeaksToKeep; ++i) { m_centerCandidatesList.push_back(merged_peaks_position_votes[i].first); m_centerVotes.push_back(static_cast(merged_peaks_position_votes[i].second)); } @@ -991,7 +992,7 @@ vpCircleHoughTransform::computeCircleCandidates() r_effective = weigthedSumRadius / votes; } return r_effective; - }; + }; #endif // Merging similar candidates @@ -1041,7 +1042,7 @@ vpCircleHoughTransform::computeCircleCandidates() ++idCandidate; } - if ((votes_effective > m_algoParams.m_centerMinThresh) && (votes_effective >= m_algoParams.m_circleVisibilityRatioThresh * 2.f * M_PIf * r_effective)) { + if ((votes_effective > m_algoParams.m_centerMinThresh) && (votes_effective >= (m_algoParams.m_circleVisibilityRatioThresh * 2.f * M_PIf * r_effective))) { // Only the circles having enough votes and being visible enough are considered v_r_effective.push_back(r_effective); v_votes_effective.push_back(votes_effective); diff --git a/modules/imgproc/src/vpConnectedComponents.cpp b/modules/imgproc/src/vpConnectedComponents.cpp index 718936c77a..63e2f24d32 100644 --- a/modules/imgproc/src/vpConnectedComponents.cpp +++ b/modules/imgproc/src/vpConnectedComponents.cpp @@ -68,12 +68,12 @@ void getNeighbors(const vpImage &I, std::queue &lis } } else { - for (int cpt1 = -1; cpt1 <= 1; cpt1++) { - for (int cpt2 = -1; cpt2 <= 1; cpt2++) { + for (int cpt1 = -1; cpt1 <= 1; ++cpt1) { + for (int cpt2 = -1; cpt2 <= 1; ++cpt2) { // Everything except the current position - if (cpt1 != 0 || cpt2 != 0) { - if (I[(int)i + cpt1][(int)j + cpt2] == currValue) { - listOfNeighbors.push(vpImagePoint((int)i + cpt1, (int)j + cpt2)); + if ((cpt1 != 0) || (cpt2 != 0)) { + if (I[static_cast(i) + cpt1][static_cast(j) + cpt2] == currValue) { + listOfNeighbors.push(vpImagePoint(static_cast(i) + cpt1, static_cast(j) + cpt2)); } } } @@ -87,8 +87,8 @@ void visitNeighbors(vpImage &I_copy, std::queue &li // Visit the neighbors while (!listOfNeighbors.empty()) { vpImagePoint imPt = listOfNeighbors.front(); - unsigned int i = (unsigned int)imPt.get_i(); - unsigned int j = (unsigned int)imPt.get_j(); + unsigned int i = static_cast(imPt.get_i()); + unsigned int j = static_cast(imPt.get_j()); listOfNeighbors.pop(); if (I_copy[i][j]) { @@ -114,8 +114,9 @@ void connectedComponents(const vpImage &I, vpImage &labels, vpImage I_copy(I.getHeight() + 2, I.getWidth() + 2); // Copy and add border - for (unsigned int i = 0; i < I_copy.getHeight(); i++) { - if (i == 0 || i == I_copy.getHeight() - 1) { + unsigned int i_copy_height = I_copy.getHeight(); + for (unsigned int i = 0; i < i_copy_height; ++i) { + if ((i == 0) || (i == (I_copy.getHeight() - 1))) { memset(I_copy[i], 0, sizeof(unsigned char) * I_copy.getWidth()); } else { @@ -130,13 +131,15 @@ void connectedComponents(const vpImage &I, vpImage &labels, int current_label = 1; std::queue listOfNeighbors; - for (unsigned int cpt1 = 0; cpt1 < I.getHeight(); cpt1++) { + unsigned int i_height = I.getHeight(); + for (unsigned int cpt1 = 0; cpt1 < i_height; ++cpt1) { unsigned int i = cpt1 + 1; - for (unsigned int cpt2 = 0; cpt2 < I.getWidth(); cpt2++) { + unsigned int i_width = I.getWidth(); + for (unsigned int cpt2 = 0; cpt2 < i_width; ++cpt2) { unsigned int j = cpt2 + 1; - if (I_copy[i][j] && labels_copy[i][j] == 0) { + if (I_copy[i][j] && (labels_copy[i][j] == 0)) { // Get all the neighbors relative to the current position getNeighbors(I_copy, listOfNeighbors, i, j, connexity); @@ -151,8 +154,8 @@ void connectedComponents(const vpImage &I, vpImage &labels, } } } - - for (unsigned int i = 0; i < labels.getHeight(); i++) { + unsigned int labels_height = labels.getHeight(); + for (unsigned int i = 0; i < labels_height; ++i) { memcpy(labels[i], labels_copy[i + 1] + 1, sizeof(int) * labels.getWidth()); } diff --git a/modules/imgproc/src/vpContours.cpp b/modules/imgproc/src/vpContours.cpp index 1084cc45ff..f7e2814c34 100644 --- a/modules/imgproc/src/vpContours.cpp +++ b/modules/imgproc/src/vpContours.cpp @@ -119,24 +119,24 @@ bool crossesEastBorder(const vpImage &I, bool checked[8], const vpImagePoin return false; } - bool b = checked[(int)direction.m_direction]; + bool b = checked[static_cast(direction.m_direction)]; - if (point.get_i() < 0 || point.get_j() < 0) { + if ((point.get_i() < 0) || (point.get_j() < 0)) { return false; } - unsigned int i = (unsigned int)point.get_i(); - unsigned int j = (unsigned int)point.get_j(); + unsigned int i = static_cast(point.get_i()); + unsigned int j = static_cast(point.get_j()); - return I[i][j] != 0 && ((unsigned int)point.get_j() == I.getWidth() - 1 || b); + return (I[i][j] != 0) && ((static_cast(point.get_j()) == (I.getWidth() - 1)) || b); } void addContourPoint(vpImage &I, vp::vpContour *border, const vpImagePoint &point, bool checked[8], int nbd) { border->m_points.push_back(vpImagePoint(point.get_i() - 1, point.get_j() - 1)); // remove 1-pixel padding - unsigned int i = (unsigned int)point.get_i(); - unsigned int j = (unsigned int)point.get_j(); + unsigned int i = static_cast(point.get_i()); + unsigned int j = static_cast(point.get_j()); if (crossesEastBorder(I, checked, point)) { I[i][j] = -nbd; @@ -157,7 +157,7 @@ void followBorder(vpImage &I, const vpImagePoint &ij, vpImagePoint &i2j2, v vpDirection trace = dir.clockwise(); vpImagePoint i1j1(-1, -1); - // Find i1j1 (3.1) + // --comment: find i1j1 (3.1) while (trace.m_direction != dir.m_direction) { vpImagePoint activePixel = trace.active(I, ij); @@ -169,7 +169,7 @@ void followBorder(vpImage &I, const vpImagePoint &ij, vpImagePoint &i2j2, v trace = trace.clockwise(); } - if (i1j1.get_i() < 0 || i1j1.get_j() < 0) { + if ((i1j1.get_i() < 0) || (i1j1.get_j() < 0)) { //(3.1) ; single pixel contour return; } @@ -188,23 +188,23 @@ void followBorder(vpImage &I, const vpImagePoint &ij, vpImagePoint &i2j2, v vpImagePoint i4j4(-1, -1); // Reset checked - for (int cpt = 0; cpt < 8; cpt++) { + for (int cpt = 0; cpt < 8; ++cpt) { checked[cpt] = false; } while (true) { i4j4 = trace.active(I, i3j3); //(3.3) - if (i4j4.get_i() >= 0 && i4j4.get_j() >= 0) { + if ((i4j4.get_i() >= 0) && (i4j4.get_j() >= 0)) { break; } - checked[(int)trace.m_direction] = true; + checked[static_cast(trace.m_direction)] = true; trace = trace.counterClockwise(); } addContourPoint(I, border, i3j3, checked, nbd); - if (i4j4 == ij && i3j3 == i1j1) { + if ((i4j4 == ij) && (i3j3 == i1j1)) { //(3.5) break; } @@ -217,17 +217,17 @@ void followBorder(vpImage &I, const vpImagePoint &ij, vpImagePoint &i2j2, v bool isOuterBorderStart(const vpImage &I, unsigned int i, unsigned int j) { - return (I[i][j] == 1 && (j == 0 || I[i][j - 1] == 0)); + return ((I[i][j] == 1) && ((j == 0) || (I[i][j - 1] == 0))); } bool isHoleBorderStart(const vpImage &I, unsigned int i, unsigned int j) { - return (I[i][j] >= 1 && (j == I.getWidth() - 1 || I[i][j + 1] == 0)); + return ((I[i][j] >= 1) && ((j == (I.getWidth() - 1)) || (I[i][j + 1] == 0))); } void getContoursList(const vp::vpContour &root, int level, vp::vpContour &contour_list) { - if (level > 0 && level < std::numeric_limits::max()) { + if ((level > 0) && (level < std::numeric_limits::max())) { vp::vpContour *contour_node = new vp::vpContour; contour_node->m_contourType = root.m_contourType; contour_node->m_points = root.m_points; @@ -251,8 +251,8 @@ 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) { - unsigned int i = (unsigned int)it2->get_i(); - unsigned int j = (unsigned int)it2->get_j(); + unsigned int i = static_cast(it2->get_i()); + unsigned int j = static_cast(it2->get_j()); I[i][j] = grayValue; } } @@ -266,8 +266,8 @@ 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) { - unsigned int i = (unsigned int)it2->get_i(); - unsigned int j = (unsigned int)it2->get_j(); + 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); } } @@ -285,16 +285,21 @@ void findContours(const vpImage &I_original, vpContour &contours, // Copy uchar I_original into int I + padding vpImage I(I_original.getHeight() + 2, I_original.getWidth() + 2); - for (unsigned int i = 0; i < I.getHeight(); i++) { - if (i == 0 || i == I.getHeight() - 1) { - memset(I.bitmap, 0, sizeof(int) * I.getWidth()); + + unsigned int i_height = I.getHeight(); + unsigned int i_width = I.getWidth(); + unsigned int i_original_width = I_original.getWidth(); + + for (unsigned int i = 0; i < i_height; ++i) { + if ((i == 0) || (i == (i_height - 1))) { + memset(I.bitmap, 0, sizeof(int) * i_width); } else { I[i][0] = 0; - for (unsigned int j = 0; j < I_original.getWidth(); j++) { + for (unsigned int j = 0; j < i_original_width; ++j) { I[i][j + 1] = I_original[i - 1][j]; } - I[i][I.getWidth() - 1] = 0; + I[i][i_width - 1] = 0; } } @@ -311,10 +316,10 @@ void findContours(const vpImage &I_original, vpContour &contours, std::map borderMap; borderMap[lnbd] = root; - for (unsigned int i = 0; i < I.getHeight(); i++) { + for (unsigned int i = 0; i < i_height; ++i) { lnbd = 1; // Reset LNBD at the beginning of each scan row - for (unsigned int j = 0; j < I.getWidth(); j++) { + for (unsigned int j = 0; j < i_width; ++j) { int fji = I[i][j]; bool isOuter = isOuterBorderStart(I, i, j); @@ -382,7 +387,7 @@ void findContours(const vpImage &I_original, vpContour &contours, I[i][j] = -nbd; } - if (retrievalMode == CONTOUR_RETR_LIST || retrievalMode == CONTOUR_RETR_TREE) { + if ((retrievalMode == CONTOUR_RETR_LIST) || (retrievalMode == CONTOUR_RETR_TREE)) { // Add contour points contourPts.push_back(border->m_points); } @@ -391,13 +396,13 @@ void findContours(const vpImage &I_original, vpContour &contours, } //(4) - if (fji != 0 && fji != 1) { + if ((fji != 0) && (fji != 1)) { lnbd = std::abs(fji); } } } - if (retrievalMode == CONTOUR_RETR_EXTERNAL || retrievalMode == CONTOUR_RETR_LIST) { + if ((retrievalMode == CONTOUR_RETR_EXTERNAL) || (retrievalMode == CONTOUR_RETR_LIST)) { // Delete contours content contours.m_parent = nullptr; @@ -424,7 +429,7 @@ void findContours(const vpImage &I_original, vpContour &contours, // Restore children (*it)->m_children = children_copy; // Set parent to children - for (size_t i = 0; i < contours.m_children.size(); i++) { + for (size_t i = 0; i < contours.m_children.size(); ++i) { contours.m_children[i]->m_parent = &contours; } contourPts.push_back((*it)->m_points); diff --git a/modules/imgproc/src/vpFloodFill.cpp b/modules/imgproc/src/vpFloodFill.cpp index cbcf1cb81e..0257338462 100644 --- a/modules/imgproc/src/vpFloodFill.cpp +++ b/modules/imgproc/src/vpFloodFill.cpp @@ -73,7 +73,7 @@ void floodFill(vpImage &I, const vpImagePoint &seedPoint, const u { // Code from Lode Vandevenne tutorial. // Naive modification for 8-connexity implementation - if (oldValue == newValue || I.getSize() == 0) { + if ((oldValue == newValue) || (I.getSize() == 0)) { return; } @@ -86,22 +86,22 @@ void floodFill(vpImage &I, const vpImagePoint &seedPoint, const u vpImagePoint current_seed = seed_queue.front(); seed_queue.pop(); - unsigned int x = (unsigned int)current_seed.get_j(); - unsigned int y = (unsigned int)current_seed.get_i(); - int x1 = (int)x; + unsigned int x = static_cast(current_seed.get_j()); + unsigned int y = static_cast(current_seed.get_i()); + int x1 = static_cast(x); // Find most left pixel - while (x1 >= 0 && I[y][x1] == oldValue) { + while ((x1 >= 0) && (I[y][x1] == oldValue)) { x1--; } x1++; bool spanAbove = false, spanBelow = false; - while (x1 < (int)I.getWidth() && I[y][x1] == oldValue) { + while ((x1 < static_cast(I.getWidth())) && (I[y][x1] == oldValue)) { I[y][x1] = newValue; - if (!spanAbove && y > 0) { + if ((!spanAbove) && (y > 0)) { if (I[y - 1][x1] == oldValue) { // North spanAbove = true; @@ -109,23 +109,23 @@ void floodFill(vpImage &I, const vpImagePoint &seedPoint, const u } if (connexity != vpImageMorphology::CONNEXITY_4) { - if (x1 > 0 && I[y - 1][x1 - 1] == oldValue) { + if ((x1 > 0) && (I[y - 1][x1 - 1] == oldValue)) { // North west spanAbove = true; seed_queue.push(vpImagePoint(y - 1, x1 - 1)); } - if (x1 < (int)I.getWidth() - 1 && I[y - 1][x1 + 1] == oldValue) { + if ((x1 < (static_cast(I.getWidth()) - 1)) && (I[y - 1][x1 + 1] == oldValue)) { // North east spanAbove = true; seed_queue.push(vpImagePoint(y - 1, x1 + 1)); } } } - else if (spanAbove && y > 0 && I[y - 1][x1] != oldValue) { + else if (spanAbove && (y > 0) && (I[y - 1][x1] != oldValue)) { spanAbove = false; } - if (!spanBelow && y < I.getHeight() - 1) { + if ((!spanBelow) && (y < (I.getHeight() - 1))) { if (I[y + 1][x1] == oldValue) { // South seed_queue.push(vpImagePoint(y + 1, x1)); @@ -133,19 +133,19 @@ void floodFill(vpImage &I, const vpImagePoint &seedPoint, const u } if (connexity != vpImageMorphology::CONNEXITY_4) { - if (x1 > 0 && I[y + 1][x1 - 1] == oldValue) { + if ((x1 > 0) && (I[y + 1][x1 - 1] == oldValue)) { // South west seed_queue.push(vpImagePoint(y + 1, x1 - 1)); spanBelow = true; } - if (x1 < (int)I.getWidth() - 1 && I[y + 1][x1 + 1] == oldValue) { + if ((x1 < (static_cast(I.getWidth()) - 1)) && (I[y + 1][x1 + 1] == oldValue)) { // South east seed_queue.push(vpImagePoint(y + 1, x1 + 1)); spanBelow = true; } } } - else if (spanBelow && y < I.getHeight() - 1 && I[y + 1][x1] != oldValue) { + else if (spanBelow && (y < (I.getHeight() - 1)) && (I[y + 1][x1] != oldValue)) { spanBelow = false; } diff --git a/modules/imgproc/src/vpImgproc.cpp b/modules/imgproc/src/vpImgproc.cpp index 0ac94de507..80ef3deac5 100644 --- a/modules/imgproc/src/vpImgproc.cpp +++ b/modules/imgproc/src/vpImgproc.cpp @@ -178,8 +178,8 @@ void adjust(vpImage &I, double alpha, double beta) { // Construct the look-up table unsigned char lut[256]; - for (unsigned int i = 0; i < 256; i++) { - lut[i] = vpMath::saturate(alpha * i + beta); + for (unsigned int i = 0; i < 256; ++i) { + lut[i] = vpMath::saturate((alpha * i) + beta); } // Apply the transformation using a LUT @@ -198,11 +198,11 @@ void adjust(vpImage &I, double alpha, double beta) { // Construct the look-up table vpRGBa lut[256]; - for (unsigned int i = 0; i < 256; i++) { - lut[i].R = vpMath::saturate(alpha * i + beta); - lut[i].G = vpMath::saturate(alpha * i + beta); - lut[i].B = vpMath::saturate(alpha * i + beta); - lut[i].A = vpMath::saturate(alpha * i + beta); + for (unsigned int i = 0; i < 256; ++i) { + lut[i].R = vpMath::saturate((alpha * i) + beta); + lut[i].G = vpMath::saturate((alpha * i) + beta); + lut[i].B = vpMath::saturate((alpha * i) + beta); + lut[i].A = vpMath::saturate((alpha * i) + beta); } // Apply the transformation using a LUT @@ -226,7 +226,7 @@ void equalizeHistogram(vpImage &I, const vpImage *p_mask) void equalizeHistogram(const vpImage &I1, vpImage &I2, const vpImage *p_mask) { - if (I1.getWidth() * I1.getHeight() == 0) { + if ((I1.getWidth() * I1.getHeight()) == 0) { return; } @@ -238,7 +238,7 @@ void equalizeHistogram(const vpImage &I1, vpImage void equalizeHistogram(vpImage &I, bool useHSV) { - if (I.getWidth() * I.getHeight() == 0) { + if ((I.getWidth() * I.getHeight()) == 0) { return; } @@ -259,7 +259,7 @@ void equalizeHistogram(vpImage &I, bool useHSV) // Merge the result in I unsigned int size = I.getWidth() * I.getHeight(); unsigned char *ptrStart = (unsigned char *)I.bitmap; - unsigned char *ptrEnd = ptrStart + size * 4; + unsigned char *ptrEnd = ptrStart + (size * 4); unsigned char *ptrCurrent = ptrStart; unsigned int cpt = 0; @@ -330,7 +330,7 @@ void gammaCorrectionLogMethod(vpImage &I, const vpImage *p_ // Construct the look-up table unsigned char lut[256]; float inputRangeAsFloat = static_cast(inputRange); - for (unsigned int i = inputMin; i <= inputMax; i++) { + for (unsigned int i = inputMin; i <= inputMax; ++i) { lut[i] = vpMath::saturate(std::pow(static_cast(i - inputMin) / inputRangeAsFloat, inverse_gamma) * 255.f); } @@ -358,13 +358,13 @@ void gammaCorrectionNonLinearMethod(vpImage &I, const vpImage(i); float phi = M_PIf * x / (2.f * x_m); float f1 = a * std::cos(phi); float k = rho * std::sin(4 * M_PIf * x / 255.f); float f2 = (k + b)*std::cos(alpha) + x * std::sin(alpha); - float r = c * std::abs(x / x_m - 1.f); + float r = c * std::abs((x / x_m) - 1.f); float f3 = r * std::cos(3.f * M_PIf * x / 255.f); float g = f1 + f2 + f3; float gamma = 1 + g; @@ -411,15 +411,15 @@ void gammaCorrectionClassificationBasedMethod(vpImage &I, const v if (meanNormalized < 0.5) { // Case dark image float meanPowerGamma = static_cast(std::pow(meanNormalized, gamma)); - for (unsigned int i = 0; i <= 255; i++) { + for (unsigned int i = 0; i <= 255; ++i) { float iNormalized = static_cast(i)/255.f; float iPowerGamma = std::pow(iNormalized, gamma); - lut[i] = vpMath::saturate(255.f * (iPowerGamma / (iPowerGamma + (1.f - iPowerGamma) * meanPowerGamma))); + lut[i] = vpMath::saturate(255.f * (iPowerGamma / (iPowerGamma + ((1.f - iPowerGamma) * meanPowerGamma)))); } } else { // Case bright image - for (unsigned int i = 0; i <= 255; i++) { + for (unsigned int i = 0; i <= 255; ++i) { float iNormalized = static_cast(i)/255.f; lut[i] = vpMath::saturate(std::pow(iNormalized, gamma) * 255.f); } @@ -463,7 +463,7 @@ void gammaCorrectionProbabilisticBased(vpImage &I, const vpImage< } unsigned char lut[256]; float cdf_w = 0; - for (unsigned int i = 0; i <= 255; i++) { + for (unsigned int i = 0; i <= 255; ++i) { cdf_w += pdf_w[i] / sum_pdf_w; float gamma = 1.f - cdf_w; float iNormalized = static_cast(i)/255.f; @@ -507,7 +507,7 @@ void gammaCorrectionSpatialBased(vpImage &I, const vpImage p = 2.f; } else if (stdev <= 80) { - p = -0.025f * stdev + 3.f; + p = (-0.025f * stdev) + 3.f; } else { p = 1.f; @@ -547,7 +547,7 @@ void gammaCorrectionSpatialBased(vpImage &I, const vpImage *p_mask vpImage I_gray(height, width); for (unsigned int i = 0; i < size; ++i) { vpRGBa rgb = I.bitmap[i]; - I_gray.bitmap[i] = static_cast(0.299 * rgb.R + 0.587 * rgb.G + 0.114 * rgb.B); + I_gray.bitmap[i] = static_cast((0.299 * rgb.R) + (0.587 * rgb.G) + (0.114 * rgb.B)); } vpImage I_2, I_4, I_8; I_gray.subsample(2, 2, I_2); @@ -572,7 +572,7 @@ void gammaCorrectionSpatialBased(vpImage &I, const vpImage *p_mask p = 2.f; } else if (stdev <= 80) { - p = -0.025f * stdev + 3.f; + p = (-0.025f * stdev) + 3.f; } else { p = 1.f; @@ -605,7 +605,7 @@ void gammaCorrection(vpImage &I, const float &gamma, const vpGamm inverse_gamma = 1.0f / gamma; // Construct the look-up table unsigned char lut[256]; - for (unsigned int i = 0; i < 256; i++) { + for (unsigned int i = 0; i < 256; ++i) { lut[i] = vpMath::saturate(std::pow(static_cast(i) / 255.0, inverse_gamma) * 255.0); } @@ -656,7 +656,7 @@ void gammaCorrection(const vpImage &I1, vpImage &I void gammaCorrection(vpImage &I, const float &gamma, const vpGammaColorHandling &colorHandling, const vpGammaMethod &method, const vpImage *p_mask) { - if ((method == GAMMA_SPATIAL_VARIANT_BASED)) { + if (method == GAMMA_SPATIAL_VARIANT_BASED) { gammaCorrectionSpatialBased(I, p_mask); } else { @@ -713,7 +713,7 @@ void stretchContrast(vpImage &I) // Construct the look-up table unsigned char lut[256]; if (range > 0) { - for (unsigned int x = min; x <= max; x++) { + for (unsigned int x = min; x <= max; ++x) { lut[x] = 255 * (x - min) / range; } } @@ -765,7 +765,7 @@ void stretchContrast(vpImage &I) vpRGBa lut[256]; unsigned char rangeR = max.R - min.R; if (rangeR > 0) { - for (unsigned int x = min.R; x <= max.R; x++) { + for (unsigned int x = min.R; x <= max.R; ++x) { lut[x].R = 255 * (x - min.R) / rangeR; } } @@ -775,7 +775,7 @@ void stretchContrast(vpImage &I) unsigned char rangeG = max.G - min.G; if (rangeG > 0) { - for (unsigned int x = min.G; x <= max.G; x++) { + for (unsigned int x = min.G; x <= max.G; ++x) { lut[x].G = 255 * (x - min.G) / rangeG; } } @@ -785,7 +785,7 @@ void stretchContrast(vpImage &I) unsigned char rangeB = max.B - min.B; if (rangeB > 0) { - for (unsigned int x = min.B; x <= max.B; x++) { + for (unsigned int x = min.B; x <= max.B; ++x) { lut[x].B = 255 * (x - min.B) / rangeB; } } @@ -795,7 +795,7 @@ void stretchContrast(vpImage &I) unsigned char rangeA = max.A - min.A; if (rangeA > 0) { - for (unsigned int x = min.A; x <= max.A; x++) { + for (unsigned int x = min.A; x <= max.A; ++x) { lut[x].A = 255 * (x - min.A) / rangeA; } } @@ -833,7 +833,7 @@ void stretchContrastHSV(vpImage &I) double *ptrCurrent = ptrStart; // Stretch Saturation - if (maxSaturation - minSaturation > 0.0) { + if ((maxSaturation - minSaturation) > 0.0) { while (ptrCurrent != ptrEnd) { *ptrCurrent = (*ptrCurrent - minSaturation) / (maxSaturation - minSaturation); ++ptrCurrent; @@ -841,7 +841,7 @@ void stretchContrastHSV(vpImage &I) } // Stretch Value - if (maxValue - minValue > 0.0) { + if ((maxValue - minValue) > 0.0) { ptrStart = valueImage.bitmap; ptrEnd = valueImage.bitmap + size; ptrCurrent = ptrStart; @@ -853,7 +853,7 @@ void stretchContrastHSV(vpImage &I) } // Convert HSV to RGBa - vpImageConvert::HSVToRGBa(hueImage.bitmap, saturationImage.bitmap, valueImage.bitmap, (unsigned char *)I.bitmap, + vpImageConvert::HSVToRGBa(hueImage.bitmap, saturationImage.bitmap, valueImage.bitmap, (unsigned char *)(I.bitmap), size); } @@ -866,7 +866,7 @@ void stretchContrastHSV(const vpImage &I1, vpImage &I2) void unsharpMask(vpImage &I, float sigma, double weight) { - if (weight < 1.0 && weight >= 0.0) { + if ((weight < 1.0) && (weight >= 0.0)) { #if defined(VISP_HAVE_SIMDLIB) // Gaussian blurred image vpGaussianFilter gaussian_filter(I.getWidth(), I.getHeight(), sigma); @@ -881,7 +881,8 @@ void unsharpMask(vpImage &I, float sigma, double weight) #endif // Unsharp mask - for (unsigned int cpt = 0; cpt < I.getSize(); cpt++) { + unsigned int i_size = I.getSize(); + for (unsigned int cpt = 0; cpt < i_size; ++cpt) { double val = (I.bitmap[cpt] - weight * I_blurred.bitmap[cpt]) / (1 - weight); I.bitmap[cpt] = vpMath::saturate(val); // val > 255 ? 255 : (val < 0 ? 0 : val); } @@ -897,7 +898,7 @@ void unsharpMask(const vpImage &I, vpImage &Ires, void unsharpMask(vpImage &I, float sigma, double weight) { - if (weight < 1.0 && weight >= 0.0) { + if ((weight < 1.0) && (weight >= 0.0)) { #if defined(VISP_HAVE_SIMDLIB) // Gaussian blurred image vpGaussianFilter gaussian_filter(I.getWidth(), I.getHeight(), sigma); @@ -917,7 +918,8 @@ void unsharpMask(vpImage &I, float sigma, double weight) #endif // Unsharp mask - for (unsigned int cpt = 0; cpt < I.getSize(); cpt++) { + unsigned int i_size = I.getSize(); + for (unsigned int cpt = 0; cpt < i_size; ++cpt) { #if defined(VISP_HAVE_SIMDLIB) double val_R = (I.bitmap[cpt].R - weight * I_blurred.bitmap[cpt].R) / (1 - weight); double val_G = (I.bitmap[cpt].G - weight * I_blurred.bitmap[cpt].G) / (1 - weight); diff --git a/modules/imgproc/src/vpMorph.cpp b/modules/imgproc/src/vpMorph.cpp index bfb8333899..c3d0435a87 100644 --- a/modules/imgproc/src/vpMorph.cpp +++ b/modules/imgproc/src/vpMorph.cpp @@ -64,17 +64,17 @@ void fillHoles(vpImage &I // - no more connexity option vpImage mask(I.getHeight() + 2, I.getWidth() + 2, 255); // Copy I to mask + add border padding + complement - for (unsigned int i = 0; i < I.getHeight(); i++) { - for (unsigned int j = 0; j < I.getWidth(); j++) { + for (unsigned int i = 0; i < I.getHeight(); ++i) { + for (unsigned int j = 0; j < I.getWidth(); ++j) { mask[i + 1][j + 1] = 255 - I[i][j]; } } vpImage marker(I.getHeight() + 2, I.getWidth() + 2, 0); // Create marker with 255 1-pixel border - for (unsigned int i = 0; i < marker.getHeight(); i++) { + for (unsigned int i = 0; i < marker.getHeight(); ++i) { if (i == 0 || i == marker.getHeight() - 1) { - for (unsigned int j = 0; j < marker.getWidth(); j++) { + for (unsigned int j = 0; j < marker.getWidth(); ++j) { marker[i][j] = 255; } } @@ -87,8 +87,8 @@ void fillHoles(vpImage &I vpImage I_reconstruct; reconstruct(marker, mask, I_reconstruct, connexity); - for (unsigned int i = 0; i < I.getHeight(); i++) { - for (unsigned int j = 0; j < I.getWidth(); j++) { + for (unsigned int i = 0; i < I.getHeight(); ++i) { + for (unsigned int j = 0; j < I.getWidth(); ++j) { I[i][j] = 255 - I_reconstruct[i + 1][j + 1]; } } @@ -96,7 +96,8 @@ void fillHoles(vpImage &I // Create flood fill mask vpImage flood_fill_mask(I.getHeight() + 2, I.getWidth() + 2, 0); // Copy I to mask + add border padding - for (unsigned int i = 0; i < I.getHeight(); i++) { + unsigned int i_height = I.getHeight(); + for (unsigned int i = 0; i < i_height; ++i) { memcpy(flood_fill_mask[i + 1] + 1, I[i], sizeof(unsigned char) * I.getWidth()); } @@ -105,7 +106,8 @@ void fillHoles(vpImage &I // Get current mask vpImage mask(I.getHeight(), I.getWidth()); - for (unsigned int i = 0; i < mask.getHeight(); i++) { + unsigned int mask_height = mask.getHeight(); + for (unsigned int i = 0; i < mask_height; ++i) { memcpy(mask[i], flood_fill_mask[i + 1] + 1, sizeof(unsigned char) * mask.getWidth()); } @@ -119,7 +121,7 @@ void fillHoles(vpImage &I void reconstruct(const vpImage &marker, const vpImage &mask, vpImage &h_kp1 /*alias I */, const vpImageMorphology::vpConnexityType &connexity) { - if (marker.getHeight() != mask.getHeight() || marker.getWidth() != mask.getWidth()) { + if ((marker.getHeight() != mask.getHeight()) || (marker.getWidth() != mask.getWidth())) { std::cerr << "marker.getHeight() != mask.getHeight() || " "marker.getWidth() != mask.getWidth()" << std::endl; @@ -139,8 +141,10 @@ void reconstruct(const vpImage &marker, const vpImage(h_kp1, connexity); // Keep min - for (unsigned int i = 0; i < h_kp1.getHeight(); i++) { - for (unsigned int j = 0; j < h_kp1.getWidth(); j++) { + unsigned int h_kp1_height = h_kp1.getHeight(); + unsigned int h_kp1_width = h_kp1.getWidth(); + for (unsigned int i = 0; i < h_kp1_height; ++i) { + for (unsigned int j = 0; j < h_kp1_width; ++j) { h_kp1[i][j] = std::min(h_kp1[i][j], mask[i][j]); } } diff --git a/modules/imgproc/src/vpRetinex.cpp b/modules/imgproc/src/vpRetinex.cpp index 12340ff9ec..2942dec19c 100644 --- a/modules/imgproc/src/vpRetinex.cpp +++ b/modules/imgproc/src/vpRetinex.cpp @@ -102,27 +102,27 @@ std::vector retinexScalesDistribution(int scaleDiv, int level, int scale scales[1] = scale; } else { - double size_step = scale / (double)scaleDiv; + double size_step = scale / static_cast(scaleDiv); int i; switch (level) { case vp::RETINEX_UNIFORM: - for (i = 0; i < scaleDiv; i++) { - scales[(size_t)i] = 2.0 + i * size_step; + for (i = 0; i < scaleDiv; ++i) { + scales[static_cast(i)] = 2.0 + (i * size_step); } break; case vp::RETINEX_LOW: - size_step = std::log(scale - 2.0) / (double)scaleDiv; - for (i = 0; i < scaleDiv; i++) { - scales[(size_t)i] = 2.0 + std::pow(10.0, (i * size_step) / std::log(10.0)); + size_step = std::log(scale - 2.0) / static_cast(scaleDiv); + for (i = 0; i < scaleDiv; ++i) { + scales[static_cast(i)] = 2.0 + std::pow(10.0, (i * size_step) / std::log(10.0)); } break; case vp::RETINEX_HIGH: - size_step = std::log(scale - 2.0) / (double)scaleDiv; - for (i = 0; i < scaleDiv; i++) { - scales[(size_t)i] = scale - std::pow(10.0, (i * size_step) / std::log(10.0)); + size_step = std::log(scale - 2.0) / static_cast(scaleDiv); + for (i = 0; i < scaleDiv; ++i) { + scales[static_cast(i)] = scale - std::pow(10.0, (i * size_step) / std::log(10.0)); } break; @@ -145,7 +145,7 @@ void MSRCR(vpImage &I, int _scale, int scaleDiv, int level, double dynam // Filtering according to the various scales. // Summarize the results of the various filters according to a specific // weight(here equivalent for all). - double weight = 1.0 / (double)scaleDiv; + double weight = 1.0 / static_cast(scaleDiv); std::vector > doubleRGB(3); std::vector > doubleResRGB(3); @@ -154,27 +154,27 @@ void MSRCR(vpImage &I, int _scale, int scaleDiv, int level, double dynam int kernelSize = _kernelSize; if (kernelSize == -1) { // Compute the kernel size from the input image size - kernelSize = (int)(std::min(I.getWidth(), I.getHeight()) / 2.0); + kernelSize = static_cast(std::min(I.getWidth(), I.getHeight()) / 2.0); kernelSize = (kernelSize - kernelSize % 2) + 1; } - for (int channel = 0; channel < 3; channel++) { - doubleRGB[(size_t)channel] = vpImage(I.getHeight(), I.getWidth()); - doubleResRGB[(size_t)channel] = vpImage(I.getHeight(), I.getWidth()); + for (int channel = 0; channel < 3; ++channel) { + doubleRGB[static_cast(channel)] = vpImage(I.getHeight(), I.getWidth()); + doubleResRGB[static_cast(channel)] = vpImage(I.getHeight(), I.getWidth()); - for (unsigned int cpt = 0; cpt < size; cpt++) { + for (unsigned int cpt = 0; cpt < size; ++cpt) { // Shift the pixel values by 1 to avoid problem with log(0) switch (channel) { case 0: - doubleRGB[(size_t)channel].bitmap[cpt] = I.bitmap[cpt].R + 1.0; + doubleRGB[static_cast(channel)].bitmap[cpt] = I.bitmap[cpt].R + 1.0; break; case 1: - doubleRGB[(size_t)channel].bitmap[cpt] = I.bitmap[cpt].G + 1.0; + doubleRGB[static_cast(channel)].bitmap[cpt] = I.bitmap[cpt].G + 1.0; break; case 2: - doubleRGB[(size_t)channel].bitmap[cpt] = I.bitmap[cpt].B + 1.0; + doubleRGB[static_cast(channel)].bitmap[cpt] = I.bitmap[cpt].B + 1.0; break; default: @@ -182,17 +182,17 @@ void MSRCR(vpImage &I, int _scale, int scaleDiv, int level, double dynam } } - for (int sc = 0; sc < scaleDiv; sc++) { + for (int sc = 0; sc < scaleDiv; ++sc) { vpImage blurImage; - double sigma = retinexScales[(size_t)sc]; - vpImageFilter::gaussianBlur(doubleRGB[(size_t)channel], blurImage, (unsigned int)kernelSize, sigma); + double sigma = retinexScales[static_cast(sc)]; + vpImageFilter::gaussianBlur(doubleRGB[static_cast(channel)], blurImage, static_cast(kernelSize), sigma); - for (unsigned int cpt = 0; cpt < size; cpt++) { + for (unsigned int cpt = 0; cpt < size; ++cpt) { // Summarize the filtered values. // In fact one calculates a ratio between the original values and the // filtered values. - doubleResRGB[(size_t)channel].bitmap[cpt] += - weight * (std::log(doubleRGB[(size_t)channel].bitmap[cpt]) - std::log(blurImage.bitmap[cpt])); + doubleResRGB[static_cast(channel)].bitmap[cpt] += + weight * (std::log(doubleRGB[static_cast(channel)].bitmap[cpt]) - std::log(blurImage.bitmap[cpt])); } } } @@ -200,14 +200,14 @@ void MSRCR(vpImage &I, int _scale, int scaleDiv, int level, double dynam std::vector dest(size * 3); const double gain = 1.0, alpha = 128.0, offset = 0.0; - for (unsigned int cpt = 0; cpt < size; cpt++) { - double logl = std::log((double)(I.bitmap[cpt].R + I.bitmap[cpt].G + I.bitmap[cpt].B + 3.0)); + for (unsigned int cpt = 0; cpt < size; ++cpt) { + double logl = std::log(static_cast(I.bitmap[cpt].R + I.bitmap[cpt].G + I.bitmap[cpt].B + 3.0)); dest[cpt * 3] = gain * (std::log(alpha * doubleRGB[0].bitmap[cpt]) - logl) * doubleResRGB[0].bitmap[cpt] + offset; - dest[cpt * 3 + 1] = - gain * (std::log(alpha * doubleRGB[1].bitmap[cpt]) - logl) * doubleResRGB[1].bitmap[cpt] + offset; - dest[cpt * 3 + 2] = - gain * (std::log(alpha * doubleRGB[2].bitmap[cpt]) - logl) * doubleResRGB[2].bitmap[cpt] + offset; + dest[(cpt * 3) + 1] = + (gain * (std::log(alpha * doubleRGB[1].bitmap[cpt]) - logl) * doubleResRGB[1].bitmap[cpt]) + offset; + dest[(cpt * 3) + 2] = + (gain * (std::log(alpha * doubleRGB[2].bitmap[cpt]) - logl) * doubleResRGB[2].bitmap[cpt]) + offset; } double sum = std::accumulate(dest.begin(), dest.end(), 0.0); @@ -224,18 +224,18 @@ void MSRCR(vpImage &I, int _scale, int scaleDiv, int level, double dynam double sq_sum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0); double stdev = std::sqrt(sq_sum / dest.size()); - double mini = mean - dynamic * stdev; - double maxi = mean + dynamic * stdev; + double mini = mean - (dynamic * stdev); + double maxi = mean + (dynamic * stdev); double range = maxi - mini; if (vpMath::nul(range)) { range = 1.0; } - for (unsigned int cpt = 0; cpt < size; cpt++) { - I.bitmap[cpt].R = vpMath::saturate((255.0 * (dest[cpt * 3 + 0] - mini) / range)); - I.bitmap[cpt].G = vpMath::saturate((255.0 * (dest[cpt * 3 + 1] - mini) / range)); - I.bitmap[cpt].B = vpMath::saturate((255.0 * (dest[cpt * 3 + 2] - mini) / range)); + for (unsigned int cpt = 0; cpt < size; ++cpt) { + I.bitmap[cpt].R = vpMath::saturate((255.0 * (dest[(cpt * 3) + 0] - mini) / range)); + I.bitmap[cpt].G = vpMath::saturate((255.0 * (dest[(cpt * 3) + 1] - mini) / range)); + I.bitmap[cpt].B = vpMath::saturate((255.0 * (dest[(cpt * 3) + 2] - mini) / range)); } } @@ -244,18 +244,18 @@ namespace vp void retinex(vpImage &I, int scale, int scaleDiv, int level, const double dynamic, int kernelSize) { // Assert scale - if (scale < 16 || scale > 250) { + if ((scale < 16) || (scale > 250)) { std::cerr << "Scale must be between the interval [16 - 250]" << std::endl; return; } // Assert scaleDiv - if (scaleDiv < 1 || scaleDiv > 8) { + if ((scaleDiv < 1) || (scaleDiv > 8)) { std::cerr << "Scale division must be between the interval [1 - 8]" << std::endl; return; } - if (I.getWidth() * I.getHeight() == 0) { + if ((I.getWidth() * I.getHeight()) == 0) { return; } diff --git a/modules/imgproc/src/vpThreshold.cpp b/modules/imgproc/src/vpThreshold.cpp index fe3fc93ab7..e9ab6d0c58 100644 --- a/modules/imgproc/src/vpThreshold.cpp +++ b/modules/imgproc/src/vpThreshold.cpp @@ -46,7 +46,8 @@ bool isBimodal(const std::vector &hist_float) { int modes = 0; - for (size_t cpt = 1; cpt < hist_float.size() - 1; cpt++) { + 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++; } @@ -71,11 +72,11 @@ int computeThresholdHuang(const vpHistogram &hist) // Find first and last non-empty bin size_t first, last; - for (first = 0; first < (size_t)hist.getSize() && hist[(unsigned char)first] == 0; first++) { + for (first = 0; (first < static_cast(hist.getSize())) && (hist[static_cast(first)] == 0); first++) { // do nothing } - for (last = (size_t)hist.getSize() - 1; last > first && hist[(unsigned char)last] == 0; last--) { + for (last = static_cast(hist.getSize()) - 1; (last > first) && (hist[static_cast(last)] == 0); last--) { // do nothing } @@ -87,42 +88,43 @@ int computeThresholdHuang(const vpHistogram &hist) std::vector S(last + 1); std::vector W(last + 1); - S[0] = (float)hist[0]; + S[0] = static_cast(hist[0]); W[0] = 0.0f; - for (size_t i = std::max((size_t)1, first); i <= last; i++) { - S[i] = S[i - 1] + hist[(unsigned char)i]; - W[i] = W[i - 1] + i * (float)hist[(unsigned char)i]; + for (size_t i = std::max(static_cast(1), first); i <= last; ++i) { + S[i] = S[i - 1] + hist[static_cast(i)]; + W[i] = W[i - 1] + (i * static_cast(hist[static_cast(i)])); } // Precalculate the summands of the entropy given the absolute difference x // - mu (integral) - float C = (float)(last - first); - std::vector Smu(last + 1 - first); + float C = static_cast(last - first); + std::vector Smu((last + 1) - first); - for (size_t i = 1; i < Smu.size(); i++) { - float mu = 1 / (1 + i / C); - Smu[i] = -mu * std::log(mu) - (1 - mu) * std::log(1 - mu); + size_t smu_size = Smu.size(); + for (size_t i = 1; i < smu_size; ++i) { + float mu = 1 / (1 + (i / C)); + Smu[i] = (-mu * std::log(mu)) - ((1 - mu) * std::log(1 - mu)); } // Calculate the threshold int bestThreshold = 0; float bestEntropy = std::numeric_limits::max(); - for (size_t threshold = first; threshold <= last; threshold++) { + for (size_t threshold = first; threshold <= last; ++threshold) { float entropy = 0; int mu = vpMath::round(W[threshold] / S[threshold]); - for (size_t i = first; i <= threshold; i++) { - entropy += Smu[(size_t)std::abs((int)i - mu)] * hist[(unsigned char)i]; + for (size_t i = first; i <= threshold; ++i) { + entropy += Smu[static_cast(std::abs(static_cast(i) - mu))] * hist[static_cast(i)]; } mu = vpMath::round((W[last] - W[threshold]) / (S[last] - S[threshold])); - for (size_t i = threshold + 1; i <= last; i++) { - entropy += Smu[(size_t)std::abs((int)i - mu)] * hist[(unsigned char)i]; + for (size_t i = threshold + 1; i <= last; ++i) { + entropy += Smu[static_cast(std::abs(static_cast(i) - mu))] * hist[static_cast(i)]; } if (bestEntropy > entropy) { bestEntropy = entropy; - bestThreshold = (int)threshold; + bestThreshold = static_cast(threshold); } } @@ -150,14 +152,16 @@ int computeThresholdIntermodes(const vpHistogram &hist) // method. std::vector hist_float(hist.getSize()); - for (unsigned int cpt = 0; cpt < hist.getSize(); cpt++) { - hist_float[cpt] = (float)hist[cpt]; + unsigned int hist_size = hist.getSize(); + for (unsigned int cpt = 0; cpt < hist_size; ++cpt) { + hist_float[cpt] = static_cast(hist[cpt]); } int iter = 0; while (!isBimodal(hist_float)) { // Smooth with a 3 point running mean filter - for (size_t cpt = 1; cpt < hist_float.size() - 1; cpt++) { + size_t hist_float_size_m_1 = hist_float.size() - 1; + for (size_t cpt = 1; cpt < hist_float_size_m_1; ++cpt) { hist_float[cpt] = (hist_float[cpt - 1] + hist_float[cpt] + hist_float[cpt + 1]) / 3; } @@ -177,14 +181,15 @@ int computeThresholdIntermodes(const vpHistogram &hist) // The threshold is the mean between the two peaks. int tt = 0; - for (size_t cpt = 1; cpt < hist_float.size() - 1; cpt++) { - if (hist_float[cpt - 1] < hist_float[cpt] && hist_float[cpt] > hist_float[cpt + 1]) { + 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])) { // Mode - tt += (int)cpt; + tt += static_cast(cpt); } } - return (int)std::floor(tt / 2.0); // vpMath::round(tt / 2.0); + return static_cast(std::floor(tt / 2.0)); // vpMath::round(tt / 2.0); } int computeThresholdIsoData(const vpHistogram &hist, unsigned int imageSize) @@ -195,24 +200,26 @@ int computeThresholdIsoData(const vpHistogram &hist, unsigned int imageSize) // STEP 1: Compute mean intensity of image from histogram, set T=mean(I) std::vector cumsum(hist.getSize(), 0.0f); std::vector sum_ip(hist.getSize(), 0.0f); - cumsum[0] = (float)hist[0]; - for (unsigned int cpt = 1; cpt < hist.getSize(); cpt++) { - sum_ip[cpt] = cpt * (float)hist[cpt] + sum_ip[cpt - 1]; - cumsum[cpt] = (float)hist[cpt] + cumsum[cpt - 1]; + cumsum[0] = static_cast(hist[0]); + + unsigned int hist_size = hist.getSize(); + for (unsigned int cpt = 1; cpt < hist_size; ++cpt) { + sum_ip[cpt] = (cpt * static_cast(hist[cpt])) + sum_ip[cpt - 1]; + cumsum[cpt] = static_cast(hist[cpt]) + cumsum[cpt - 1]; } int T = vpMath::round(sum_ip[255] / imageSize); // STEP 2: compute Mean above T (MAT) and Mean below T (MBT) using T from - float MBT = sum_ip[(size_t)(T - 2)] / cumsum[(size_t)(T - 2)]; - float MAT = (sum_ip.back() - sum_ip[(size_t)(T - 1)]) / (cumsum.back() - cumsum[(size_t)(T - 1)]); + float MBT = sum_ip[static_cast(T - 2)] / cumsum[static_cast(T - 2)]; + float MAT = (sum_ip.back() - sum_ip[static_cast(T - 1)]) / (cumsum.back() - cumsum[static_cast(T - 1)]); int T2 = vpMath::round((MAT + MBT) / 2.0f); //% STEP 3 to n: repeat step 2 if T(i)~=T(i-1) while (std::abs(T2 - T) >= 1) { - MBT = sum_ip[(size_t)(T2 - 2)] / cumsum[(size_t)(T2 - 2)]; - MAT = (sum_ip.back() - sum_ip[(size_t)(T2 - 1)]) / (cumsum.back() - cumsum[(size_t)(T2 - 1)]); + MBT = sum_ip[static_cast(T2 - 2)] / cumsum[static_cast(T2 - 2)]; + MAT = (sum_ip.back() - sum_ip[static_cast(T2 - 1)]) / (cumsum.back() - cumsum[static_cast(T2 - 1)]); T = T2; T2 = vpMath::round((MAT + MBT) / 2.0f); @@ -228,11 +235,12 @@ int computeThresholdMean(const vpHistogram &hist, unsigned int imageSize) // CVGIP: Graphical Models and Image Processing, vol. 55, pp. 532-537, 1993. // The threshold is the mean of the greyscale data float sum_ip = 0.0f; - for (unsigned int cpt = 0; cpt < hist.getSize(); cpt++) { - sum_ip += cpt * (float)hist[cpt]; + unsigned int hist_size = hist.getSize(); + for (unsigned int cpt = 0; cpt < hist_size; ++cpt) { + sum_ip += cpt * static_cast(hist[cpt]); } - return (int)std::floor(sum_ip / imageSize); + return static_cast(std::floor(sum_ip / imageSize)); } int computeThresholdOtsu(const vpHistogram &hist, unsigned int imageSize) @@ -243,8 +251,9 @@ int computeThresholdOtsu(const vpHistogram &hist, unsigned int imageSize) float mu_T = 0.0f; float sum_ip_all[256]; - for (int cpt = 0; cpt < (int)hist.getSize(); cpt++) { - mu_T += cpt * (float)hist[cpt]; + int hist_size = static_cast(hist.getSize()); + for (int cpt = 0; cpt < hist_size; ++cpt) { + mu_T += cpt * static_cast(hist[cpt]); sum_ip_all[cpt] = mu_T; } @@ -254,20 +263,20 @@ int computeThresholdOtsu(const vpHistogram &hist, unsigned int imageSize) float max_sigma_b = 0.0f; int threshold = 0; - for (int cpt = 0; cpt < 256; cpt++) { + for (int cpt = 0; cpt < 256; ++cpt) { w_B += hist[cpt]; if (vpMath::nul(w_B, std::numeric_limits::epsilon())) { continue; } - w_F = (int)imageSize - w_B; + w_F = static_cast(imageSize) - w_B; if (vpMath::nul(w_F, std::numeric_limits::epsilon())) { break; } // Mean Background / Foreground - float mu_B = sum_ip_all[cpt] / (float)w_B; - float mu_F = (mu_T - sum_ip_all[cpt]) / (float)w_F; + float mu_B = sum_ip_all[cpt] / static_cast(w_B); + float mu_F = (mu_T - sum_ip_all[cpt]) / static_cast(w_F); // If there is a case where (w_B * w_F) exceed FLT_MAX, normalize // histogram float sigma_b_sqr = w_B * w_F * (mu_B - mu_F) * (mu_B - mu_F); @@ -291,40 +300,42 @@ int computeThresholdTriangle(vpHistogram &hist) int left_bound = -1, right_bound = -1, max_idx = -1, max_value = 0; // Find max value index and left / right most index - for (int cpt = 0; cpt < (int)hist.getSize(); cpt++) { - if (left_bound == -1 && hist[cpt] > 0) { - left_bound = (int)cpt; + int hist_size = static_cast(hist.getSize()); + for (int cpt = 0; cpt < hist_size; ++cpt) { + if ((left_bound == -1) && (hist[cpt] > 0)) { + left_bound = static_cast(cpt); } - if (right_bound == -1 && hist[(int)hist.getSize() - 1 - cpt] > 0) { - right_bound = (int)hist.getSize() - 1 - cpt; + if ((right_bound == -1) && (hist[static_cast(hist.getSize()) - 1 - cpt] > 0)) { + right_bound = static_cast(hist.getSize()) - 1 - cpt; } - if ((int)hist[cpt] > max_value) { - max_value = (int)hist[cpt]; + if (static_cast(hist[cpt]) > max_value) { + max_value = static_cast(hist[cpt]); max_idx = cpt; } } // First / last index when hist(cpt) == 0 left_bound = left_bound > 0 ? left_bound - 1 : left_bound; - right_bound = right_bound < (int)hist.getSize() - 1 ? right_bound + 1 : right_bound; + right_bound = right_bound < static_cast(hist.getSize()) - 1 ? right_bound + 1 : right_bound; // Use the largest bound bool flip = false; - if (max_idx - left_bound < right_bound - max_idx) { + if ((max_idx - left_bound) < (right_bound - max_idx)) { // Flip histogram to get the largest bound to the left flip = true; - int cpt_left = 0, cpt_right = (int)hist.getSize() - 1; + int cpt_left = 0; + int cpt_right = static_cast(hist.getSize()) - 1; 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); } - left_bound = (int)hist.getSize() - 1 - right_bound; - max_idx = (int)hist.getSize() - 1 - max_idx; + left_bound = static_cast(hist.getSize()) - 1 - right_bound; + max_idx = static_cast(hist.getSize()) - 1 - max_idx; } // Distance from a point to a line defined by two points: @@ -334,12 +345,12 @@ int computeThresholdTriangle(vpHistogram &hist) // { \sqrt{ \left ( y_2 - y_1 \right )^{2} + \left ( x_2 - x_1 \right // )^{2} } } // Constants are ignored - float a = (float)max_value; // y_2 - y_1 - float b = (float)(left_bound - max_idx); //-(x_2 - x_1) + float a = static_cast(max_value); // y_2 - y_1 + float b = static_cast(left_bound - max_idx); //-(x_2 - x_1) float max_dist = 0.0f; - for (int cpt = left_bound + 1; cpt <= max_idx; cpt++) { - float dist = a * cpt + b * hist[cpt]; + for (int cpt = left_bound + 1; cpt <= max_idx; ++cpt) { + float dist = (a * cpt) + (b * hist[cpt]); if (dist > max_dist) { max_dist = dist; @@ -349,7 +360,7 @@ int computeThresholdTriangle(vpHistogram &hist) threshold--; if (flip) { - threshold = (int)hist.getSize() - 1 - threshold; + threshold = static_cast(hist.getSize()) - 1 - threshold; } return threshold; @@ -400,7 +411,7 @@ unsigned char autoThreshold(vpImage &I, const vpAutoThresholdMeth if (threshold != -1) { // Threshold - vpImageTools::binarise(I, (unsigned char)threshold, (unsigned char)255, backgroundValue, foregroundValue, + vpImageTools::binarise(I, static_cast(threshold), static_cast(255), backgroundValue, foregroundValue, foregroundValue); } diff --git a/modules/io/src/image/private/vpImageIoLibjpeg.cpp b/modules/io/src/image/private/vpImageIoLibjpeg.cpp index ecd4aa39e8..223b7fc4ea 100644 --- a/modules/io/src/image/private/vpImageIoLibjpeg.cpp +++ b/modules/io/src/image/private/vpImageIoLibjpeg.cpp @@ -96,7 +96,7 @@ void writeJPEGLibjpeg(const vpImage &I, const std::string &filena line = new unsigned char[width]; unsigned char *input = (unsigned char *)I.bitmap; while (cinfo.next_scanline < cinfo.image_height) { - for (unsigned int i = 0; i < width; i++) { + for (unsigned int i = 0; i < width; ++i) { line[i] = *(input); input++; } @@ -155,7 +155,7 @@ void writeJPEGLibjpeg(const vpImage &I, const std::string &filename, int line = new unsigned char[3 * width]; unsigned char *input = (unsigned char *)I.bitmap; while (cinfo.next_scanline < cinfo.image_height) { - for (unsigned int i = 0; i < width; i++) { + for (unsigned int i = 0; i < width; ++i) { line[i * 3] = *(input); input++; line[i * 3 + 1] = *(input); @@ -227,11 +227,11 @@ void readJPEGLibjpeg(vpImage &I, const std::string &filename) unsigned char *output = (unsigned char *)Ic.bitmap; while (cinfo.output_scanline < cinfo.output_height) { jpeg_read_scanlines(&cinfo, buffer, 1); - for (unsigned int i = 0; i < width; i++) { - *(output++) = buffer[0][i * 3]; - *(output++) = buffer[0][i * 3 + 1]; - *(output++) = buffer[0][i * 3 + 2]; - *(output++) = vpRGBa::alpha_default; + for (unsigned int i = 0; i < width; ++i) { + *(++output) = buffer[0][i * 3]; + *(++output) = buffer[0][i * 3 + 1]; + *(++output) = buffer[0][i * 3 + 2]; + *(++output) = vpRGBa::alpha_default; } } vpImageConvert::convert(Ic, I); @@ -307,11 +307,11 @@ void readJPEGLibjpeg(vpImage &I, const std::string &filename) unsigned char *output = (unsigned char *)I.bitmap; while (cinfo.output_scanline < cinfo.output_height) { jpeg_read_scanlines(&cinfo, buffer, 1); - for (unsigned int i = 0; i < width; i++) { - *(output++) = buffer[0][i * 3]; - *(output++) = buffer[0][i * 3 + 1]; - *(output++) = buffer[0][i * 3 + 2]; - *(output++) = vpRGBa::alpha_default; + for (unsigned int i = 0; i < width; ++i) { + *(++output) = buffer[0][i * 3]; + *(++output) = buffer[0][i * 3 + 1]; + *(++output) = buffer[0][i * 3 + 2]; + *(++output) = vpRGBa::alpha_default; } } } diff --git a/modules/io/src/image/private/vpImageIoLibpng.cpp b/modules/io/src/image/private/vpImageIoLibpng.cpp index e3a8c4fc02..884c4752f6 100644 --- a/modules/io/src/image/private/vpImageIoLibpng.cpp +++ b/modules/io/src/image/private/vpImageIoLibpng.cpp @@ -119,14 +119,14 @@ void writePNGLibpng(const vpImage &I, const std::string &filename png_write_info(png_ptr, info_ptr); png_bytep *row_ptrs = new png_bytep[height]; - for (unsigned int i = 0; i < height; i++) + for (unsigned int i = 0; i < height; ++i) row_ptrs[i] = new png_byte[width]; unsigned char *input = (unsigned char *)I.bitmap; - for (unsigned int i = 0; i < height; i++) { + for (unsigned int i = 0; i < height; ++i) { png_byte *row = row_ptrs[i]; - for (unsigned int j = 0; j < width; j++) { + for (unsigned int j = 0; j < width; ++j) { row[j] = *(input); input++; } @@ -136,7 +136,7 @@ void writePNGLibpng(const vpImage &I, const std::string &filename png_write_end(png_ptr, nullptr); - for (unsigned int j = 0; j < height; j++) + for (unsigned int j = 0; j < height; ++j) delete[] row_ptrs[j]; delete[] row_ptrs; @@ -216,14 +216,14 @@ void writePNGLibpng(const vpImage &I, const std::string &filename) png_write_info(png_ptr, info_ptr); png_bytep *row_ptrs = new png_bytep[height]; - for (unsigned int i = 0; i < height; i++) + for (unsigned int i = 0; i < height; ++i) row_ptrs[i] = new png_byte[3 * width]; unsigned char *input = (unsigned char *)I.bitmap; - for (unsigned int i = 0; i < height; i++) { + for (unsigned int i = 0; i < height; ++i) { png_byte *row = row_ptrs[i]; - for (unsigned int j = 0; j < width; j++) { + for (unsigned int j = 0; j < width; ++j) { row[3 * j] = *(input); input++; row[3 * j + 1] = *(input); @@ -238,7 +238,7 @@ void writePNGLibpng(const vpImage &I, const std::string &filename) png_write_end(png_ptr, nullptr); - for (unsigned int j = 0; j < height; j++) + for (unsigned int j = 0; j < height; ++j) delete[] row_ptrs[j]; delete[] row_ptrs; @@ -369,7 +369,7 @@ void readPNGLibpng(vpImage &I, const std::string &filename) unsigned int stride = png_get_rowbytes(png_ptr, info_ptr); unsigned char *data = new unsigned char[stride * height]; - for (unsigned int i = 0; i < height; i++) + for (unsigned int i = 0; i < height; ++i) rowPtrs[i] = (png_bytep)data + (i * stride); png_read_image(png_ptr, rowPtrs); @@ -380,36 +380,36 @@ void readPNGLibpng(vpImage &I, const std::string &filename) switch (channels) { case 1: output = (unsigned char *)I.bitmap; - for (unsigned int i = 0; i < width * height; i++) { - *(output++) = data[i]; + for (unsigned int i = 0; i < width * height; ++i) { + *(++output) = data[i]; } break; case 2: output = (unsigned char *)I.bitmap; - for (unsigned int i = 0; i < width * height; i++) { - *(output++) = data[i * 2]; + for (unsigned int i = 0; i < width * height; ++i) { + *(++output) = data[i * 2]; } break; case 3: output = (unsigned char *)Ic.bitmap; - for (unsigned int i = 0; i < width * height; i++) { - *(output++) = data[i * 3]; - *(output++) = data[i * 3 + 1]; - *(output++) = data[i * 3 + 2]; - *(output++) = vpRGBa::alpha_default; + for (unsigned int i = 0; i < width * height; ++i) { + *(++output) = data[i * 3]; + *(++output) = data[i * 3 + 1]; + *(++output) = data[i * 3 + 2]; + *(++output) = vpRGBa::alpha_default; } vpImageConvert::convert(Ic, I); break; case 4: output = (unsigned char *)Ic.bitmap; - for (unsigned int i = 0; i < width * height; i++) { - *(output++) = data[i * 4]; - *(output++) = data[i * 4 + 1]; - *(output++) = data[i * 4 + 2]; - *(output++) = data[i * 4 + 3]; + for (unsigned int i = 0; i < width * height; ++i) { + *(++output) = data[i * 4]; + *(++output) = data[i * 4 + 1]; + *(++output) = data[i * 4 + 2]; + *(++output) = data[i * 4 + 3]; } vpImageConvert::convert(Ic, I); break; @@ -546,7 +546,7 @@ void readPNGLibpng(vpImage &I, const std::string &filename) unsigned int stride = png_get_rowbytes(png_ptr, info_ptr); unsigned char *data = new unsigned char[stride * height]; - for (unsigned int i = 0; i < height; i++) + for (unsigned int i = 0; i < height; ++i) rowPtrs[i] = (png_bytep)data + (i * stride); png_read_image(png_ptr, rowPtrs); @@ -557,37 +557,37 @@ void readPNGLibpng(vpImage &I, const std::string &filename) switch (channels) { case 1: output = (unsigned char *)Ig.bitmap; - for (unsigned int i = 0; i < width * height; i++) { - *(output++) = data[i]; + for (unsigned int i = 0; i < width * height; ++i) { + *(++output) = data[i]; } vpImageConvert::convert(Ig, I); break; case 2: output = (unsigned char *)Ig.bitmap; - for (unsigned int i = 0; i < width * height; i++) { - *(output++) = data[i * 2]; + for (unsigned int i = 0; i < width * height; ++i) { + *(++output) = data[i * 2]; } vpImageConvert::convert(Ig, I); break; case 3: output = (unsigned char *)I.bitmap; - for (unsigned int i = 0; i < width * height; i++) { - *(output++) = data[i * 3]; - *(output++) = data[i * 3 + 1]; - *(output++) = data[i * 3 + 2]; - *(output++) = vpRGBa::alpha_default; + for (unsigned int i = 0; i < width * height; ++i) { + *(++output) = data[i * 3]; + *(++output) = data[i * 3 + 1]; + *(++output) = data[i * 3 + 2]; + *(++output) = vpRGBa::alpha_default; } break; case 4: output = (unsigned char *)I.bitmap; - for (unsigned int i = 0; i < width * height; i++) { - *(output++) = data[i * 4]; - *(output++) = data[i * 4 + 1]; - *(output++) = data[i * 4 + 2]; - *(output++) = data[i * 4 + 3]; + for (unsigned int i = 0; i < width * height; ++i) { + *(++output) = data[i * 4]; + *(++output) = data[i * 4 + 1]; + *(++output) = data[i * 4 + 2]; + *(++output) = data[i * 4 + 3]; } break; } diff --git a/modules/io/src/image/private/vpImageIoPortable.cpp b/modules/io/src/image/private/vpImageIoPortable.cpp index 7960632c44..1158483ef6 100644 --- a/modules/io/src/image/private/vpImageIoPortable.cpp +++ b/modules/io/src/image/private/vpImageIoPortable.cpp @@ -90,13 +90,15 @@ void vp_decodeHeaderPNM(const std::string &filename, std::ifstream &fd, const st cpt_elt++; header.erase(header.begin(), header.begin() + 1); // erase first element that is processed - } else if (cpt_elt == 2) { // decode height + } + else if (cpt_elt == 2) { // decode height std::istringstream ss(header[0]); ss >> h; cpt_elt++; header.erase(header.begin(), header.begin() + 1); // erase first element that is processed - } else if (cpt_elt == 3) { // decode maxval + } + else if (cpt_elt == 3) { // decode maxval std::istringstream ss(header[0]); ss >> maxval; cpt_elt++; @@ -148,13 +150,15 @@ void vp_decodeHeaderPFM(const std::string &filename, std::ifstream &fd, std::str cpt_elt++; header.erase(header.begin(), header.begin() + 1); // erase first element that is processed - } else if (cpt_elt == 2) { // decode height + } + else if (cpt_elt == 2) { // decode height std::istringstream ss(header[0]); ss >> h; cpt_elt++; header.erase(header.begin(), header.begin() + 1); // erase first element that is processed - } else if (cpt_elt == 3) { // decode byte order + } + else if (cpt_elt == 3) { // decode byte order std::istringstream ss(header[0]); ss >> scale; littleEndian = scale < 0; @@ -275,7 +279,7 @@ void vp_writePFM_HDR(const vpImage &I, const std::string &filename) size_t nbyte = I.getWidth() * 3; for (int i = static_cast(I.getHeight()) - 1; i >= 0; i--) { size_t ierr = fwrite(I[i], sizeof(float), nbyte, fd); - if (ierr != nbyte) { + if (ierr != nbyte) { fclose(fd); throw(vpImageException(vpImageException::ioError, "Cannot save PFM file \"%s\": only %d bytes over %d saved ", filename.c_str(), ierr, nbyte)); @@ -347,7 +351,7 @@ void vp_writePGM(const vpImage &I, const std::string &filename) Iuc.resize(nrows, ncols); - for (unsigned int i = 0; i < nrows * ncols; i++) + for (unsigned int i = 0; i < nrows * ncols; ++i) Iuc.bitmap[i] = (unsigned char)I.bitmap[i]; vp_writePGM(Iuc, filename); @@ -503,7 +507,7 @@ void vp_readPFM_HDR(vpImage &I, const std::string &filename) for (int i = I.getHeight() - 1; i >= 0; i--) { fd.read((char *)I[i], sizeof(float) * w * channels); if (swapEndianness) { - for (unsigned int j = 0; j < w * channels; j++) { + for (unsigned int j = 0; j < w * channels; ++j) { I[i][j] = vpEndian::swapFloat(I[i][j]); } } @@ -517,8 +521,8 @@ void vp_readPFM_HDR(vpImage &I, const std::string &filename) fd.close(); if (std::fabs(scale) > 0.0f) { - for (unsigned int i = 0; i < I.getHeight(); i++) { - for (unsigned int j = 0; j < I.getWidth(); j++) { + for (unsigned int i = 0; i < I.getHeight(); ++i) { + for (unsigned int j = 0; j < I.getWidth(); ++j) { I[i][j] *= 1.0f / static_cast(std::fabs(scale)); } } @@ -578,7 +582,7 @@ void vp_readPFM_HDR(vpImage &I, const std::string &filename) for (int i = I.getHeight() - 1; i >= 0; i--) { fd.read((char *)I[i], sizeof(float) * w * channels); if (swapEndianness) { - for (unsigned int j = 0; j < w; j++) { + for (unsigned int j = 0; j < w; ++j) { I[i][j].R = vpEndian::swapFloat(I[i][j].R); I[i][j].G = vpEndian::swapFloat(I[i][j].G); I[i][j].B = vpEndian::swapFloat(I[i][j].B); @@ -594,8 +598,8 @@ void vp_readPFM_HDR(vpImage &I, const std::string &filename) fd.close(); if (std::fabs(scale) > 0.0f) { - for (unsigned int i = 0; i < I.getHeight(); i++) { - for (unsigned int j = 0; j < I.getWidth(); j++) { + for (unsigned int i = 0; i < I.getHeight(); ++i) { + for (unsigned int j = 0; j < I.getWidth(); ++j) { I[i][j].R *= 1.0f / static_cast(std::fabs(scale)); I[i][j].G *= 1.0f / static_cast(std::fabs(scale)); I[i][j].B *= 1.0f / static_cast(std::fabs(scale)); @@ -750,8 +754,8 @@ void vp_readPPM(vpImage &I, const std::string &filename) I.resize(h, w); } - for (unsigned int i = 0; i < I.getHeight(); i++) { - for (unsigned int j = 0; j < I.getWidth(); j++) { + for (unsigned int i = 0; i < I.getHeight(); ++i) { + for (unsigned int j = 0; j < I.getWidth(); ++j) { unsigned char rgb[3]; fd.read((char *)&rgb, 3); @@ -814,8 +818,8 @@ void vp_writePPM(const vpImage &I, const std::string &filename) fprintf(f, "%u %u\n", I.getWidth(), I.getHeight()); // Image size fprintf(f, "%d\n", 255); // Max level - for (unsigned int i = 0; i < I.getHeight(); i++) { - for (unsigned int j = 0; j < I.getWidth(); j++) { + for (unsigned int i = 0; i < I.getHeight(); ++i) { + for (unsigned int j = 0; j < I.getWidth(); ++j) { vpRGBa v = I[i][j]; unsigned char rgb[3]; rgb[0] = v.R; diff --git a/modules/io/src/image/private/vpImageIoSimd.cpp b/modules/io/src/image/private/vpImageIoSimd.cpp index 3c63abaa85..261af7012f 100644 --- a/modules/io/src/image/private/vpImageIoSimd.cpp +++ b/modules/io/src/image/private/vpImageIoSimd.cpp @@ -49,7 +49,7 @@ void readSimdlib(vpImage &I, const std::string &filename) uint8_t *data = SimdImageLoadFromFile(filename.c_str(), &stride, &width, &height, &format); // Since the Simd lib use aligned data, some padding are introduced and we need to take care of it when copying I.init(static_cast(height), static_cast(width)); - for (size_t i = 0; i < height; i++) { + for (size_t i = 0; i < height; ++i) { memcpy(reinterpret_cast(I.bitmap) + i * width, data + i * stride, width); } SimdFree(data); @@ -62,7 +62,7 @@ void readSimdlib(vpImage &I, const std::string &filename) uint8_t *data = SimdImageLoadFromFile(filename.c_str(), &stride, &width, &height, &format); // Since the Simd lib use aligned data, some padding are introduced and we need to take care of it when copying I.init(static_cast(height), static_cast(width)); - for (size_t i = 0; i < height; i++) { + for (size_t i = 0; i < height; ++i) { memcpy(reinterpret_cast(I.bitmap) + i * width * 4, data + i * stride, 4 * width); } SimdFree(data); diff --git a/modules/io/src/image/private/vpImageIoStb.cpp b/modules/io/src/image/private/vpImageIoStb.cpp index 55c6699977..5db5a9f3bd 100644 --- a/modules/io/src/image/private/vpImageIoStb.cpp +++ b/modules/io/src/image/private/vpImageIoStb.cpp @@ -132,7 +132,7 @@ static void custom_stbi_write_mem(void *context, void *data, int size) char *dst = (char *)c->context; char *src = (char *)data; int cur_pos = c->last_pos; - for (int i = 0; i < size; i++) { + for (int i = 0; i < size; ++i) { dst[cur_pos++] = src[i]; } c->last_pos = cur_pos; diff --git a/modules/io/src/image/private/vpImageIoTinyEXR.cpp b/modules/io/src/image/private/vpImageIoTinyEXR.cpp index add5590864..d26e5078aa 100644 --- a/modules/io/src/image/private/vpImageIoTinyEXR.cpp +++ b/modules/io/src/image/private/vpImageIoTinyEXR.cpp @@ -77,7 +77,7 @@ void readEXRTiny(vpImage &I, const std::string &filename) } // Read HALF channel as FLOAT. - for (int i = 0; i < exr_header.num_channels; i++) { + for (int i = 0; i < exr_header.num_channels; ++i) { if (exr_header.pixel_types[i] == TINYEXR_PIXELTYPE_HALF) { exr_header.requested_pixel_types[i] = TINYEXR_PIXELTYPE_FLOAT; } @@ -105,14 +105,14 @@ void readEXRTiny(vpImage &I, const std::string &filename) I.resize(exr_image.height, exr_image.width); size_t data_width = static_cast(exr_header.data_window.max_x - exr_header.data_window.min_x + 1); - for (int tile_idx = 0; tile_idx < exr_image.num_tiles; tile_idx++) { + for (int tile_idx = 0; tile_idx < exr_image.num_tiles; ++tile_idx) { int sx = exr_image.tiles[tile_idx].offset_x * exr_header.tile_size_x; int sy = exr_image.tiles[tile_idx].offset_y * exr_header.tile_size_y; int ex = exr_image.tiles[tile_idx].offset_x * exr_header.tile_size_x + exr_image.tiles[tile_idx].width; int ey = exr_image.tiles[tile_idx].offset_y * exr_header.tile_size_y + exr_image.tiles[tile_idx].height; - for (unsigned int y = 0; y < static_cast(ey - sy); y++) { - for (unsigned int x = 0; x < static_cast(ex - sx); x++) { + for (unsigned int y = 0; y < static_cast(ey - sy); ++y) { + for (unsigned int x = 0; x < static_cast(ex - sx); ++x) { const float *src_image = reinterpret_cast(exr_image.tiles[tile_idx].images[0]); I.bitmap[(y + sy) * data_width + (x + sx)] = src_image[y * exr_header.tile_size_x + x]; } @@ -150,7 +150,7 @@ void readEXRTiny(vpImage &I, const std::string &filename) } // Read HALF channel as FLOAT. - for (int i = 0; i < exr_header.num_channels; i++) { + for (int i = 0; i < exr_header.num_channels; ++i) { if (exr_header.pixel_types[i] == TINYEXR_PIXELTYPE_HALF) { exr_header.requested_pixel_types[i] = TINYEXR_PIXELTYPE_FLOAT; } @@ -172,8 +172,8 @@ void readEXRTiny(vpImage &I, const std::string &filename) // `exr_image.tiled` will be filled when EXR is tiled format. if (exr_image.images) { I.resize(exr_image.height, exr_image.width); - for (int i = 0; i < exr_image.height; i++) { - for (int j = 0; j < exr_image.width; j++) { + for (int i = 0; i < exr_image.height; ++i) { + for (int j = 0; j < exr_image.width; ++j) { I[i][j].R = reinterpret_cast(exr_image.images)[2][i * exr_image.width + j]; I[i][j].G = reinterpret_cast(exr_image.images)[1][i * exr_image.width + j]; I[i][j].B = reinterpret_cast(exr_image.images)[0][i * exr_image.width + j]; @@ -184,24 +184,24 @@ void readEXRTiny(vpImage &I, const std::string &filename) I.resize(exr_image.height, exr_image.width); size_t data_width = static_cast(exr_header.data_window.max_x - exr_header.data_window.min_x + 1); - for (int tile_idx = 0; tile_idx < exr_image.num_tiles; tile_idx++) { + for (int tile_idx = 0; tile_idx < exr_image.num_tiles; ++tile_idx) { int sx = exr_image.tiles[tile_idx].offset_x * exr_header.tile_size_x; int sy = exr_image.tiles[tile_idx].offset_y * exr_header.tile_size_y; int ex = exr_image.tiles[tile_idx].offset_x * exr_header.tile_size_x + exr_image.tiles[tile_idx].width; int ey = exr_image.tiles[tile_idx].offset_y * exr_header.tile_size_y + exr_image.tiles[tile_idx].height; - //for (size_t c = 0; c < static_cast(exr_header.num_channels); c++) { + //for (size_t c = 0; c < static_cast(exr_header.num_channels); ++c) { // const float *src_image = reinterpret_cast(exr_image.tiles[tile_idx].images[c]); - // for (size_t y = 0; y < static_cast(ey - sy); y++) { - // for (size_t x = 0; x < static_cast(ex - sx); x++) { + // for (size_t y = 0; y < static_cast(ey - sy); ++y) { + // for (size_t x = 0; x < static_cast(ex - sx); ++x) { // reinterpret_cast(I.bitmap)[(y + sy) * data_width * 3 + (x + sx) * 3 + c] = src_image[y * exr_header.tile_size_x + x]; // } // } //} - for (unsigned int y = 0; y < static_cast(ey - sy); y++) { - for (unsigned int x = 0; x < static_cast(ex - sx); x++) { - for (unsigned int c = 0; c < 3; c++) { + for (unsigned int y = 0; y < static_cast(ey - sy); ++y) { + for (unsigned int x = 0; x < static_cast(ex - sx); ++x) { + for (unsigned int c = 0; c < 3; ++c) { const float *src_image = reinterpret_cast(exr_image.tiles[tile_idx].images[c]); reinterpret_cast(I.bitmap)[(y + sy) * data_width * 3 + (x + sx) * 3 + c] = src_image[y * exr_header.tile_size_x + x]; } @@ -236,7 +236,7 @@ void writeEXRTiny(const vpImage &I, const std::string &filename) header.pixel_types = (int *)malloc(sizeof(int) * header.num_channels); header.requested_pixel_types = (int *)malloc(sizeof(int) * header.num_channels); header.compression_type = TINYEXR_COMPRESSIONTYPE_ZIP; - for (int i = 0; i < header.num_channels; i++) { + for (int i = 0; i < header.num_channels; ++i) { header.pixel_types[i] = TINYEXR_PIXELTYPE_FLOAT; // pixel type of input image header.requested_pixel_types[i] = TINYEXR_PIXELTYPE_FLOAT; // pixel type of output image to be stored in .EXR } @@ -273,7 +273,7 @@ void writeEXRTiny(const vpImage &I, const std::string &filename) images[2].resize(I.getSize()); // Split RGBRGBRGB... into R, G and B layer - for (unsigned int i = 0; i < I.getSize(); i++) { + for (unsigned int i = 0; i < I.getSize(); ++i) { images[0][i] = I.bitmap[i].R; images[1][i] = I.bitmap[i].G; images[2][i] = I.bitmap[i].B; @@ -298,7 +298,7 @@ void writeEXRTiny(const vpImage &I, const std::string &filename) header.pixel_types = (int *)malloc(sizeof(int) * header.num_channels); header.requested_pixel_types = (int *)malloc(sizeof(int) * header.num_channels); header.compression_type = TINYEXR_COMPRESSIONTYPE_ZIP; - for (int i = 0; i < header.num_channels; i++) { + for (int i = 0; i < header.num_channels; ++i) { header.pixel_types[i] = TINYEXR_PIXELTYPE_FLOAT; // pixel type of input image header.requested_pixel_types[i] = TINYEXR_PIXELTYPE_FLOAT; // pixel type of output image to be stored in .EXR } diff --git a/modules/io/src/tools/vpParseArgv.cpp b/modules/io/src/tools/vpParseArgv.cpp index 856646a0b3..5dcc3755c1 100644 --- a/modules/io/src/tools/vpParseArgv.cpp +++ b/modules/io/src/tools/vpParseArgv.cpp @@ -113,13 +113,13 @@ bool vpParseArgv::parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, i */ matchPtr = nullptr; - for (unsigned int i = 0; i < 2; i++) { + for (unsigned int i = 0; i < 2; ++i) { if (i == 0) { infoPtr = argTable; } else { infoPtr = defaultTable; } - for (; infoPtr->type != ARGV_END; infoPtr++) { + for (; infoPtr->type != ARGV_END; ++infoPtr) { if (infoPtr->key == nullptr) { continue; } @@ -172,7 +172,7 @@ bool vpParseArgv::parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, i nargs = (uintptr_t)infoPtr->src; if (nargs < 1) nargs = 1; - for (unsigned long i = 0; i < nargs; i++) { + for (unsigned long i = 0; i < nargs; ++i) { if (argc == 0) { goto missingArg; } else { @@ -192,7 +192,7 @@ bool vpParseArgv::parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, i nargs = (uintptr_t)infoPtr->src; if (nargs < 1) nargs = 1; - for (unsigned long i = 0; i < nargs; i++) { + for (unsigned long i = 0; i < nargs; ++i) { if (argc == 0) { goto missingArg; } else { @@ -212,7 +212,7 @@ bool vpParseArgv::parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, i nargs = (uintptr_t)infoPtr->src; if (nargs < 1) nargs = 1; - for (unsigned long i = 0; i < nargs; i++) { + for (unsigned long i = 0; i < nargs; ++i) { if (argc == 0) { goto missingArg; } else { @@ -229,7 +229,7 @@ bool vpParseArgv::parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, i nargs = (uintptr_t)infoPtr->src; if (nargs < 1) nargs = 1; - for (unsigned long i = 0; i < nargs; i++) { + for (unsigned long i = 0; i < nargs; ++i) { if (argc == 0) { goto missingArg; } else { @@ -250,7 +250,7 @@ bool vpParseArgv::parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, i nargs = (uintptr_t)infoPtr->src; if (nargs < 1) nargs = 1; - for (unsigned long i = 0; i < nargs; i++) { + for (unsigned long i = 0; i < nargs; ++i) { if (argc == 0) { goto missingArg; } else { @@ -354,8 +354,8 @@ void vpParseArgv::printUsage(vpArgvInfo *argTable, int flags) */ width = 4; - for (unsigned int i = 0; i < 2; i++) { - for (infoPtr = i ? defaultTable : argTable; infoPtr->type != ARGV_END; infoPtr++) { + for (unsigned int i = 0; i < 2; ++i) { + for (infoPtr = i ? defaultTable : argTable; infoPtr->type != ARGV_END; ++infoPtr) { int length; if (infoPtr->key == nullptr) { continue; @@ -368,8 +368,8 @@ void vpParseArgv::printUsage(vpArgvInfo *argTable, int flags) } FPRINTF(stderr, "Command-specific options:"); - for (unsigned int i = 0;; i++) { - for (infoPtr = i ? defaultTable : argTable; infoPtr->type != ARGV_END; infoPtr++) { + for (unsigned int i = 0;; ++i) { + for (infoPtr = i ? defaultTable : argTable; infoPtr->type != ARGV_END; ++infoPtr) { if ((infoPtr->type == ARGV_HELP) && (infoPtr->key == nullptr)) { FPRINTF(stderr, "\n%s", infoPtr->help); continue; @@ -391,7 +391,7 @@ void vpParseArgv::printUsage(vpArgvInfo *argTable, int flags) nargs = (uintptr_t)infoPtr->src; if (nargs < 1) nargs = 1; - for (unsigned long j = 0; j < nargs; j++) { + for (unsigned long j = 0; j < nargs; ++j) { FPRINTF(stderr, " %d", *(((int *)infoPtr->dst) + j)); } break; @@ -401,7 +401,7 @@ void vpParseArgv::printUsage(vpArgvInfo *argTable, int flags) nargs = (uintptr_t)infoPtr->src; if (nargs < 1) nargs = 1; - for (unsigned long j = 0; j < nargs; j++) { + for (unsigned long j = 0; j < nargs; ++j) { FPRINTF(stderr, " %ld", *(((long *)infoPtr->dst) + j)); } break; @@ -411,7 +411,7 @@ void vpParseArgv::printUsage(vpArgvInfo *argTable, int flags) nargs = (uintptr_t)infoPtr->src; if (nargs < 1) nargs = 1; - for (unsigned long j = 0; j < nargs; j++) { + for (unsigned long j = 0; j < nargs; ++j) { FPRINTF(stderr, " %f", *(((float *)infoPtr->dst) + j)); } break; @@ -421,7 +421,7 @@ void vpParseArgv::printUsage(vpArgvInfo *argTable, int flags) nargs = (uintptr_t)infoPtr->src; if (nargs < 1) nargs = 1; - for (unsigned long j = 0; j < nargs; j++) { + for (unsigned long j = 0; j < nargs; ++j) { FPRINTF(stderr, " %g", *(((double *)infoPtr->dst) + j)); } break; @@ -435,7 +435,7 @@ void vpParseArgv::printUsage(vpArgvInfo *argTable, int flags) string = *((const char **)infoPtr->dst); if ((nargs == 1) && (string == nullptr)) break; - for (unsigned long j = 0; j < nargs; j++) { + for (unsigned long j = 0; j < nargs; ++j) { string = *(((const char **)infoPtr->dst) + j); if (string != nullptr) { FPRINTF(stderr, " \"%s\"", string); diff --git a/modules/io/src/video/vpVideoReader.cpp b/modules/io/src/video/vpVideoReader.cpp index 571a489490..600a63695c 100644 --- a/modules/io/src/video/vpVideoReader.cpp +++ b/modules/io/src/video/vpVideoReader.cpp @@ -711,7 +711,7 @@ void vpVideoReader::findLastFrameIndex() } std::vector files = vpIoTools::getDirFiles(dirName); m_lastFrame = 0; - for (size_t i = 0; i < files.size(); i++) { + for (size_t i = 0; i < files.size(); ++i) { // Checking that file name satisfies image format, // specified by imageNameFormat, and extracting imageIndex long imageIndex = vpIoTools::getIndex(files[i], imageNameFormat); @@ -763,7 +763,7 @@ void vpVideoReader::findFirstFrameIndex() } std::vector files = vpIoTools::getDirFiles(dirName); m_firstFrame = -1; - for (size_t i = 0; i < files.size(); i++) { + for (size_t i = 0; i < files.size(); ++i) { // Checking that file name satisfies image format, specified by // imageNameFormat, and extracting imageIndex long imageIndex = vpIoTools::getIndex(files[i], imageNameFormat); @@ -873,7 +873,7 @@ bool vpVideoReader::checkImageNameFormat(const std::string &format) const if (indexBegin == std::string::npos || indexEnd == std::string::npos) { return false; } - for (size_t i = indexBegin + 1; i < indexEnd; i++) { + for (size_t i = indexBegin + 1; i < indexEnd; ++i) { if (!std::isdigit(format[i])) { return false; } diff --git a/modules/tracker/blob/src/dots/vpDot.cpp b/modules/tracker/blob/src/dots/vpDot.cpp index ae2f972a6d..7a3ce539c9 100644 --- a/modules/tracker/blob/src/dots/vpDot.cpp +++ b/modules/tracker/blob/src/dots/vpDot.cpp @@ -344,7 +344,7 @@ bool vpDot::connexe(const vpImage &I, unsigned int u, unsigned in ip_edges_list.push_back(ip); if (graphics == true) { vpImagePoint ip_(ip); - for (unsigned int t = 0; t < thickness; t++) { + for (unsigned int t = 0; t < thickness; ++t) { ip_.set_u(ip.get_u() + t); vpDisplay::displayPoint(I, ip_, vpColor::red); } @@ -406,9 +406,9 @@ void vpDot::COG(const vpImage &I, double &u, double &v) mean_gray_level, u_cog, v_cog, npoint) == vpDot::out) { bool sol = false; unsigned int pas; - for (pas = 2; pas <= 25; pas++)if (sol==false) { - for (int k = -1; k <=1; k++) if (sol==false) - for (int l = -1; l <=1; l++) if (sol==false) { + for (pas = 2; pas <= 25; ++pas)if (sol==false) { + for (int k = -1; k <=1; ++k) if (sol==false) + for (int l = -1; l <=1; ++l) if (sol==false) { u_cog = 0; v_cog = 0; ip_connexities_list.clear(); @@ -441,7 +441,7 @@ void vpDot::COG(const vpImage &I, double &u, double &v) // Spiral search from the center to find the nearest dot while ((right < SPIRAL_SEARCH_SIZE) && (sol == false)) { - for (k = 1; k <= right; k++) + for (k = 1; k <= right; ++k) if (sol == false) { u_cog = 0; v_cog = 0; @@ -458,7 +458,7 @@ void vpDot::COG(const vpImage &I, double &u, double &v) u_ += k; right += 2; - for (k = 1; k <= botom; k++) + for (k = 1; k <= botom; ++k) if (sol == false) { u_cog = 0; v_cog = 0; @@ -476,7 +476,7 @@ void vpDot::COG(const vpImage &I, double &u, double &v) v_ += k; botom += 2; - for (k = 1; k <= left; k++) + for (k = 1; k <= left; ++k) if (sol == false) { u_cog = 0; v_cog = 0; @@ -494,7 +494,7 @@ void vpDot::COG(const vpImage &I, double &u, double &v) u_ -= k; left += 2; - for (k = 1; k <= up; k++) + for (k = 1; k <= up; ++k) if (sol == false) { u_cog = 0; v_cog = 0; diff --git a/modules/tracker/blob/src/dots/vpDot2.cpp b/modules/tracker/blob/src/dots/vpDot2.cpp index be938889d4..ccf57ba60f 100644 --- a/modules/tracker/blob/src/dots/vpDot2.cpp +++ b/modules/tracker/blob/src/dots/vpDot2.cpp @@ -41,8 +41,6 @@ \brief Track a dot. */ -//#define DEBUG - #include // exception handling @@ -84,10 +82,20 @@ void vpDot2::init() ellipsoidShapePrecision = 0.65; maxSizeSearchDistancePrecision = 0.65; setEllipsoidBadPointsPercentage(); - m00 = m11 = m02 = m20 = m10 = m01 = 0.; - mu11 = mu02 = mu20 = 0.; - - bbox_u_min = bbox_u_max = bbox_v_min = bbox_v_max = 0; + m00 = 0.; + m11 = 0.; + m02 = 0.; + m20 = 0.; + m10 = 0.; + m01 = 0.; + mu11 = 0.; + mu02 = 0.; + mu20 = 0.; + + bbox_u_min = 0; + bbox_u_max = 0; + bbox_v_min = 0; + bbox_v_max = 0; firstBorder_u = 0; firstBorder_v = 0; @@ -153,7 +161,7 @@ vpDot2 &vpDot2::operator=(const vpDot2 &twinDot) mean_gray_level = twinDot.mean_gray_level; grayLevelPrecision = twinDot.grayLevelPrecision; gamma = twinDot.gamma; - ; + sizePrecision = twinDot.sizePrecision; ellipsoidShapePrecision = twinDot.ellipsoidShapePrecision; maxSizeSearchDistancePrecision = twinDot.maxSizeSearchDistancePrecision; @@ -203,7 +211,7 @@ vpDot2 &vpDot2::operator=(const vpDot2 &twinDot) */ void vpDot2::display(const vpImage &I, vpColor color, unsigned int t) const { - vpDisplay::displayCross(I, cog, 3 * t + 8, color, t); + 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) { @@ -247,22 +255,24 @@ void vpDot2::initTracking(const vpImage &I, unsigned int size) while (vpDisplay::getClick(I, cog) != true) { } - unsigned int i = (unsigned int)cog.get_i(); - unsigned int j = (unsigned int)cog.get_j(); + unsigned int i = static_cast(cog.get_i()); + unsigned int j = static_cast(cog.get_j()); - double Ip = pow((double)I[i][j] / 255, 1 / gamma); + double Ip = pow(static_cast(I[i][j] / 255), 1 / gamma); - if (Ip - (1 - grayLevelPrecision) < 0) { + if ((Ip - (1 - grayLevelPrecision)) < 0) { gray_level_min = 0; } else { - gray_level_min = (unsigned int)(255 * pow(Ip - (1 - grayLevelPrecision), gamma)); - if (gray_level_min > 255) + gray_level_min = static_cast(255 * pow(Ip - (1 - grayLevelPrecision), gamma)); + if (gray_level_min > 255) { gray_level_min = 255; + } } - gray_level_max = (unsigned int)(255 * pow(Ip + (1 - grayLevelPrecision), gamma)); - if (gray_level_max > 255) + gray_level_max = static_cast(255 * pow(Ip + (1 - grayLevelPrecision), gamma)); + if (gray_level_max > 255) { gray_level_max = 255; + } setWidth(size); setHeight(size); @@ -271,8 +281,7 @@ void vpDot2::initTracking(const vpImage &I, unsigned int size) track(I); } catch (const vpException &e) { - // vpERROR_TRACE("Error caught") ; - throw(e); + throw e; } } @@ -307,22 +316,24 @@ void vpDot2::initTracking(const vpImage &I, const vpImagePoint &i { cog = ip; - unsigned int i = (unsigned int)cog.get_i(); - unsigned int j = (unsigned int)cog.get_j(); + unsigned int i = static_cast(cog.get_i()); + unsigned int j = static_cast(cog.get_j()); - double Ip = pow((double)I[i][j] / 255, 1 / gamma); + double Ip = pow(static_cast(I[i][j]) / 255, 1 / gamma); - if (Ip - (1 - grayLevelPrecision) < 0) { + if ((Ip - (1 - grayLevelPrecision)) < 0) { gray_level_min = 0; } else { - gray_level_min = (unsigned int)(255 * pow(Ip - (1 - grayLevelPrecision), gamma)); - if (gray_level_min > 255) + gray_level_min = static_cast(255 * pow(Ip - (1 - grayLevelPrecision), gamma)); + if (gray_level_min > 255) { gray_level_min = 255; + } } - gray_level_max = (unsigned int)(255 * pow(Ip + (1 - grayLevelPrecision), gamma)); - if (gray_level_max > 255) + gray_level_max = static_cast(255 * pow(Ip + (1 - grayLevelPrecision), gamma)); + if (gray_level_max > 255) { gray_level_max = 255; + } setWidth(size); setHeight(size); @@ -331,8 +342,7 @@ void vpDot2::initTracking(const vpImage &I, const vpImagePoint &i track(I); } catch (const vpException &e) { - // vpERROR_TRACE("Error caught") ; - throw(e); + throw e; } } @@ -390,8 +400,7 @@ void vpDot2::initTracking(const vpImage &I, const vpImagePoint &i track(I); } catch (const vpException &e) { - // vpERROR_TRACE("Error caught") ; - throw(e); + throw e; } } @@ -438,7 +447,12 @@ void vpDot2::initTracking(const vpImage &I, const vpImagePoint &i */ void vpDot2::track(const vpImage &I, bool canMakeTheWindowGrow) { - m00 = m11 = m02 = m20 = m10 = m01 = 0; + m00 = 0; + m11 = 0; + m02 = 0; + m20 = 0; + m10 = 0; + m01 = 0; // First, we will estimate the position of the tracked point @@ -452,9 +466,6 @@ void vpDot2::track(const vpImage &I, bool canMakeTheWindowGrow) // this copy to set the current found dot to the previous one (see below). vpDot2 wantedDot(*this); - // vpDEBUG_TRACE(0, "Previous dot: "); - // vpDEBUG_TRACE(0, "u: %f v: %f", get_u(), get_v()); - // vpDEBUG_TRACE(0, "w: %f h: %f", getWidth(), getHeight()); bool found = computeParameters(I, cog.get_u(), cog.get_v()); if (found) { @@ -478,10 +489,11 @@ void vpDot2::track(const vpImage &I, bool canMakeTheWindowGrow) // get the first element in the area. // first get the size of the search window from the dot size - double searchWindowWidth = 0.0, searchWindowHeight = 0.0; - // if( getWidth() == 0 || getHeight() == 0 ) - if (std::fabs(getWidth()) <= std::numeric_limits::epsilon() || - std::fabs(getHeight()) <= std::numeric_limits::epsilon()) { + double searchWindowWidth = 0.0; + double searchWindowHeight = 0.0; + + if ((std::fabs(getWidth()) <= std::numeric_limits::epsilon()) || + (std::fabs(getHeight()) <= std::numeric_limits::epsilon())) { searchWindowWidth = 80.; searchWindowHeight = 80.; } @@ -495,9 +507,10 @@ void vpDot2::track(const vpImage &I, bool canMakeTheWindowGrow) } std::list candidates; - searchDotsInArea(I, (int)(this->cog.get_u() - searchWindowWidth / 2.0), - (int)(this->cog.get_v() - searchWindowHeight / 2.0), (unsigned int)searchWindowWidth, - (unsigned int)searchWindowHeight, candidates); + searchDotsInArea(I, static_cast(this->cog.get_u() - (searchWindowWidth / 2.0)), + static_cast(this->cog.get_v() - (searchWindowHeight / 2.0)), + static_cast(searchWindowWidth), + static_cast(searchWindowHeight), candidates); // if the vector is empty, that mean we didn't find any candidate // in the area, return an error tracking. @@ -544,25 +557,24 @@ void vpDot2::track(const vpImage &I, bool canMakeTheWindowGrow) double Ip = pow(getMeanGrayLevel() / 255, 1 / gamma); // printf("current value of gray level center : %i\n", I[v][u]); - // getMeanGrayLevel(I); - if (Ip - (1 - grayLevelPrecision) < 0) { + // get Mean Gray Level of I + if ((Ip - (1 - grayLevelPrecision)) < 0) { gray_level_min = 0; } else { - gray_level_min = (unsigned int)(255 * pow(Ip - (1 - grayLevelPrecision), gamma)); - if (gray_level_min > 255) + gray_level_min = static_cast(255 * pow(Ip - (1 - grayLevelPrecision), gamma)); + if (gray_level_min > 255) { gray_level_min = 255; + } } - gray_level_max = (unsigned int)(255 * pow(Ip + (1 - grayLevelPrecision), gamma)); - if (gray_level_max > 255) + gray_level_max = static_cast(255 * pow(Ip + (1 - grayLevelPrecision), gamma)); + if (gray_level_max > 255) { gray_level_max = 255; + } - // printf("%i %i \n",gray_level_max,gray_level_min); if (graphics) { // display a red cross at the center of gravity's location in the image. - - vpDisplay::displayCross(I, this->cog, 3 * thickness + 8, vpColor::red, thickness); - // vpDisplay::flush(I); + vpDisplay::displayCross(I, this->cog, (3 * thickness) + 8, vpColor::red, thickness); } } @@ -658,7 +670,7 @@ double vpDot2::getDistance(const vpDot2 &distantDot) const vpImagePoint cogDistantDot = distantDot.getCog(); double diff_u = this->cog.get_u() - cogDistantDot.get_u(); double diff_v = this->cog.get_v() - cogDistantDot.get_v(); - return sqrt(diff_u * diff_u + diff_v * diff_v); + return sqrt((diff_u * diff_u) + (diff_v * diff_v)); } ///// SET METHODS //////////////////////////////////////////////////////////// @@ -861,19 +873,25 @@ void vpDot2::setArea(const vpImage &I, int u, int v, unsigned int unsigned int image_h = I.getHeight(); // Bounds the area to the image - if (u < 0) + if (u < 0) { u = 0; - else if (u >= (int)image_w) - u = (int)image_w - 1; - if (v < 0) + } + else if (u >= static_cast(image_w)) { + u = static_cast(image_w) - 1; + } + if (v < 0) { v = 0; - else if (v >= (int)image_h) - v = (int)image_h - 1; + } + else if (v >= static_cast(image_h)) { + v = static_cast(image_h) - 1; + } - if (((unsigned int)u + w) > image_w) - w = image_w - (unsigned int)u - 1; - if (((unsigned int)v + h) > image_h) - h = image_h - (unsigned int)v - 1; + if ((static_cast(u) + w) > image_w) { + w = image_w - static_cast(u) - 1; + } + if ((static_cast(v) + h) > image_h) { + h = image_h - static_cast(v) - 1; + } area.setRect(u, v, w, h); } @@ -986,7 +1004,6 @@ void vpDot2::searchDotsInArea(const vpImage &I, int area_u, int a if (graphics) { // Display the area were the dot is search vpDisplay::displayRectangle(I, area, vpColor::blue, false, thickness); - // vpDisplay::flush(I); } #ifdef DEBUG @@ -1003,10 +1020,10 @@ void vpDot2::searchDotsInArea(const vpImage &I, int area_u, int a vpDot2 *dotToTest = nullptr; vpDot2 tmpDot; - unsigned int area_u_min = (unsigned int)area.getLeft(); - unsigned int area_u_max = (unsigned int)area.getRight(); - unsigned int area_v_min = (unsigned int)area.getTop(); - unsigned int area_v_max = (unsigned int)area.getBottom(); + unsigned int area_u_min = static_cast(area.getLeft()); + unsigned int area_u_max = static_cast(area.getRight()); + unsigned int area_v_min = static_cast(area.getTop()); + unsigned int area_v_max = static_cast(area.getBottom()); unsigned int u, v; vpImagePoint cogTmpDot; @@ -1024,7 +1041,7 @@ void vpDot2::searchDotsInArea(const vpImage &I, int area_u, int a bool good_germ = true; itnice = niceDots.begin(); - while (itnice != niceDots.end() && good_germ == true) { + while ((itnice != niceDots.end()) && (good_germ == true)) { tmpDot = *itnice; cogTmpDot = tmpDot.getCog(); @@ -1033,7 +1050,7 @@ void vpDot2::searchDotsInArea(const vpImage &I, int area_u, int a double half_w = tmpDot.getWidth() / 2.; double half_h = tmpDot.getHeight() / 2.; - if (u >= (u0 - half_w) && u <= (u0 + half_w) && v >= (v0 - half_h) && v <= (v0 + half_h)) { + if ((u >= (u0 - half_w)) && (u <= (u0 + half_w)) && (v >= (v0 - half_h)) && (v <= (v0 + half_h))) { // Germ is in a previously detected dot good_germ = false; } @@ -1060,15 +1077,14 @@ void vpDot2::searchDotsInArea(const vpImage &I, int area_u, int a vpImagePoint cogBadDot; while (itbad != badDotsVector.end() && good_germ == true) { - if ((double)u >= vpBAD_DOT_VALUE.bbox_u_min && (double)u <= vpBAD_DOT_VALUE.bbox_u_max && - (double)v >= vpBAD_DOT_VALUE.bbox_v_min && (double)v <= vpBAD_DOT_VALUE.bbox_v_max) { + if ((static_cast(u) >= vpBAD_DOT_VALUE.bbox_u_min) && (static_cast(u) <= vpBAD_DOT_VALUE.bbox_u_max) && + (static_cast(v) >= vpBAD_DOT_VALUE.bbox_v_min) && (static_cast(v) <= vpBAD_DOT_VALUE.bbox_v_max)) { std::list::const_iterator it_edges = ip_edges_list.begin(); while (it_edges != ip_edges_list.end() && good_germ == true) { // Test if the germ belong to a previously detected dot: // - from the germ go right to the border and compare this // position to the list of pixels of previously detected dots cogBadDot = *it_edges; - // if( border_u == cogBadDot.get_u() && v == cogBadDot.get_v()) { if ((std::fabs(border_u - cogBadDot.get_u()) <= vpMath::maximum(std::fabs((double)border_u), std::fabs(cogBadDot.get_u())) * std::numeric_limits::epsilon()) && @@ -1134,17 +1150,17 @@ void vpDot2::searchDotsInArea(const vpImage &I, int area_u, int a // area_center_v) but the center of the input area which may be // partially outside the image. - double area_center_u = area_u + area_w / 2.0 - 0.5; - double area_center_v = area_v + area_h / 2.0 - 0.5; + double area_center_u = (area_u + (area_w / 2.0)) - 0.5; + double area_center_v = (area_v + (area_h / 2.0)) - 0.5; double thisDiff_u = cogDotToTest.get_u() - area_center_u; double thisDiff_v = cogDotToTest.get_v() - area_center_v; - double thisDist = sqrt(thisDiff_u * thisDiff_u + thisDiff_v * thisDiff_v); + double thisDist = sqrt((thisDiff_u * thisDiff_u) + (thisDiff_v * thisDiff_v)); bool stopLoop = false; itnice = niceDots.begin(); - while (itnice != niceDots.end() && stopLoop == false) { + while ((itnice != niceDots.end()) && (stopLoop == false)) { tmpDot = *itnice; // double epsilon = 0.001; // detecte +sieurs points @@ -1153,8 +1169,8 @@ void vpDot2::searchDotsInArea(const vpImage &I, int area_u, int a // don't add it, test the next point of the grid cogTmpDot = tmpDot.getCog(); - if (fabs(cogTmpDot.get_u() - cogDotToTest.get_u()) < epsilon && - fabs(cogTmpDot.get_v() - cogDotToTest.get_v()) < epsilon) { + if ((fabs(cogTmpDot.get_u() - cogDotToTest.get_u()) < epsilon) && + (fabs(cogTmpDot.get_v() - cogDotToTest.get_v()) < epsilon)) { stopLoop = true; // Jump all the pixels between v,u and v, // tmpDot->getFirstBorder_u() @@ -1165,7 +1181,7 @@ void vpDot2::searchDotsInArea(const vpImage &I, int area_u, int a double otherDiff_u = cogTmpDot.get_u() - area_center_u; double otherDiff_v = cogTmpDot.get_v() - area_center_v; - double otherDist = sqrt(otherDiff_u * otherDiff_u + otherDiff_v * otherDiff_v); + double otherDist = sqrt((otherDiff_u * otherDiff_u) + (otherDiff_v * otherDiff_v)); // if the distance of the curent vector element to the center // is greater than the distance of this dot to the center, @@ -1186,7 +1202,7 @@ void vpDot2::searchDotsInArea(const vpImage &I, int area_u, int a // if we reached the end of the vector without finding the dot // or inserting it, insert it now. - if (itnice == niceDots.end() && stopLoop == false) { + if ((itnice == niceDots.end()) && (stopLoop == false)) { niceDots.push_back(*dotToTest); } } @@ -1235,9 +1251,7 @@ bool vpDot2::isValid(const vpImage &I, const vpDot2 &wantedDot) // && (wantedDot.getArea() != 0) ) if ((std::fabs(wantedDot.getWidth()) > std::numeric_limits::epsilon()) && (std::fabs(wantedDot.getHeight()) > std::numeric_limits::epsilon()) && - (std::fabs(wantedDot.getArea()) > std::numeric_limits::epsilon())) - // if (size_precision!=0){ - { + (std::fabs(wantedDot.getArea()) > std::numeric_limits::epsilon())) { if (std::fabs(size_precision) > std::numeric_limits::epsilon()) { double epsilon = 0.001; #ifdef DEBUG @@ -1314,7 +1328,7 @@ bool vpDot2::isValid(const vpImage &I, const vpDot2 &wantedDot) double step_angle = 2 * M_PI / nb_point_to_test; // if (ellipsoidShape_precision != 0 && compute_moment) { - if (std::fabs(ellipsoidShape_precision) > std::numeric_limits::epsilon() && compute_moment) { + if ((std::fabs(ellipsoidShape_precision) > std::numeric_limits::epsilon()) && compute_moment) { // Chaumette, Image Moments: A General and Useful Set of Features for Visual Servoing, TRO 2004, eq 15 // mu11 = m11 - m00 * xg * yg = m11 - m00 * m10/m00 * m01/m00 @@ -1329,12 +1343,12 @@ bool vpDot2::isValid(const vpImage &I, const vpDot2 &wantedDot) // a2^2 = 2 / m00 * (mu02 + mu20 - sqrt( (mu20 - mu02)^2 + 4mu11^2) ) // we compute parameters of the estimated ellipse - double tmp1 = (m01 * m01 - m10 * m10) / m00 + (m20 - m02); - double tmp2 = m11 - m10 * m01 / m00; - double Sqrt = sqrt(tmp1 * tmp1 + 4 * tmp2 * tmp2); - double a1 = sqrt(2 / m00 * ((m20 + m02) - (m10 * m10 + m01 * m01) / m00 + Sqrt)); - double a2 = sqrt(2 / m00 * ((m20 + m02) - (m10 * m10 + m01 * m01) / m00 - Sqrt)); - double alpha = 0.5 * atan2(2 * (m11 * m00 - m10 * m01), ((m20 - m02) * m00 - m10 * m10 + m01 * m01)); + double tmp1 = (((m01 * m01) - (m10 * m10)) / m00) + (m20 - m02); + double tmp2 = (m11 - m10) * (m01 / m00); + double Sqrt = sqrt((tmp1 * tmp1) + (4 * tmp2 * tmp2)); + double a1 = sqrt((2 / m00) * ((m20 + m02) - (((m10 * m10) + (m01 * m01)) / m00) + Sqrt)); + double a2 = sqrt((2 / m00) * ((m20 + m02) - (((m10 * m10) + (m01 * m01)) / m00) - Sqrt)); + double alpha = 0.5 * atan2(2 * ((m11 * m00) - (m10 * m01)), (((m20 - m02) * m00) - (m10 * m10) + (m01 * m01))); // to be able to track small dots, minorize the ellipsoid radius for the // inner test @@ -1349,19 +1363,18 @@ bool vpDot2::isValid(const vpImage &I, const vpDot2 &wantedDot) vpImagePoint ip; nb_bad_points = 0; for (double theta = 0.; theta < 2 * M_PI; theta += step_angle) { - u = (unsigned int)(cog_u + innerCoef * (a1 * cos(alpha) * cos(theta) - a2 * sin(alpha) * sin(theta))); - v = (unsigned int)(cog_v + innerCoef * (a1 * sin(alpha) * cos(theta) + a2 * cos(alpha) * sin(theta))); + u = static_cast(cog_u + innerCoef * (a1 * cos(alpha) * cos(theta) - a2 * sin(alpha) * sin(theta))); + v = static_cast(cog_v + innerCoef * (a1 * sin(alpha) * cos(theta) + a2 * cos(alpha) * sin(theta))); if (!this->hasGoodLevel(I, u, v)) { #ifdef DEBUG printf("Inner circle pixel (%u, %u) has bad level for dot (%g, %g): " "%d not in [%u, %u]\n", u, v, cog_u, cog_v, I[v][u], gray_level_min, gray_level_max); #endif - // return false; nb_bad_points++; } if (graphics) { - for (unsigned int t = 0; t < thickness; t++) { + for (unsigned int t = 0; t < thickness; ++t) { ip.set_u(u + t); ip.set_v(v); vpDisplay::displayPoint(I, ip, vpColor::green); @@ -1386,8 +1399,8 @@ bool vpDot2::isValid(const vpImage &I, const vpDot2 &wantedDot) double outCoef = 2 - ellipsoidShape_precision; // 1.6; nb_bad_points = 0; for (double theta = 0.; theta < 2 * M_PI; theta += step_angle) { - u = (unsigned int)(cog_u + outCoef * (a1 * cos(alpha) * cos(theta) - a2 * sin(alpha) * sin(theta))); - v = (unsigned int)(cog_v + outCoef * (a1 * sin(alpha) * cos(theta) + a2 * cos(alpha) * sin(theta))); + u = static_cast(cog_u + outCoef * (a1 * cos(alpha) * cos(theta) - a2 * sin(alpha) * sin(theta))); + v = static_cast(cog_v + outCoef * (a1 * sin(alpha) * cos(theta) + a2 * cos(alpha) * sin(theta))); #ifdef DEBUG // vpDisplay::displayRectangle(I, area, vpColor::yellow); vpDisplay::displayCross(I, (int)v, (int)u, 7, vpColor::purple); @@ -1405,10 +1418,9 @@ bool vpDot2::isValid(const vpImage &I, const vpDot2 &wantedDot) u, v, cog_u, cog_v, I[v][u], gray_level_min, gray_level_max); #endif nb_bad_points++; - // return false; } if (graphics) { - for (unsigned int t = 0; t < thickness; t++) { + for (unsigned int t = 0; t < thickness; ++t) { ip.set_u(u + t); ip.set_v(v); @@ -1447,10 +1459,11 @@ bool vpDot2::isValid(const vpImage &I, const vpDot2 &wantedDot) */ bool vpDot2::hasGoodLevel(const vpImage &I, const unsigned int &u, const unsigned int &v) const { - if (!isInArea(u, v)) + if (!isInArea(u, v)) { return false; + } - if (I[v][u] >= gray_level_min && I[v][u] <= gray_level_max) { + if ((I[v][u] >= gray_level_min) && (I[v][u] <= gray_level_max)) { return true; } else { @@ -1473,10 +1486,11 @@ bool vpDot2::hasGoodLevel(const vpImage &I, const unsigned int &u bool vpDot2::hasReverseLevel(const vpImage &I, const unsigned int &u, const unsigned int &v) const { - if (!isInArea(u, v)) + if (!isInArea(u, v)) { return false; + } - if (I[v][u] < gray_level_min || I[v][u] > gray_level_max) { + if ((I[v][u] < gray_level_min) || (I[v][u] > gray_level_max)) { return true; } else { @@ -1558,29 +1572,29 @@ bool vpDot2::computeParameters(const vpImage &I, const double &_u // if u has default value, set it to the actual center value // if( est_u == -1.0 ) - if (std::fabs(est_u + 1.0) <= vpMath::maximum(std::fabs(est_u), 1.) * std::numeric_limits::epsilon()) { + if (std::fabs(est_u + 1.0) <= (vpMath::maximum(std::fabs(est_u), 1.) * std::numeric_limits::epsilon())) { est_u = this->cog.get_u(); } // if v has default value, set it to the actual center value // if( est_v == -1.0 ) - if (std::fabs(est_v + 1.0) <= vpMath::maximum(std::fabs(est_v), 1.) * std::numeric_limits::epsilon()) { + if (std::fabs(est_v + 1.0) <= (vpMath::maximum(std::fabs(est_v), 1.) * std::numeric_limits::epsilon())) { est_v = this->cog.get_v(); } // if the estimated position of the dot is out of the image, not need to // continue, return an error tracking - if (!isInArea((unsigned int)est_u, (unsigned int)est_v)) { + if (!isInArea(static_cast(est_u), static_cast(est_v))) { vpDEBUG_TRACE(3, "Initial pixel coordinates (%d, %d) for dot tracking are " "not in the area", - (int)est_u, (int)est_v); + static_cast(est_u), static_cast(est_v)); return false; } - bbox_u_min = (int)I.getWidth(); + bbox_u_min = static_cast(I.getWidth()); bbox_u_max = 0; - bbox_v_min = (int)I.getHeight(); + bbox_v_min = static_cast(I.getHeight()); bbox_v_max = 0; // if the first point doesn't have the right level then there's no point to @@ -1592,9 +1606,9 @@ bool vpDot2::computeParameters(const vpImage &I, const double &_u // find the border - if (!findFirstBorder(I, (unsigned int)est_u, (unsigned int)est_v, this->firstBorder_u, this->firstBorder_v)) { + if (!findFirstBorder(I, static_cast(est_u), static_cast(est_v), this->firstBorder_u, this->firstBorder_v)) { - vpDEBUG_TRACE(3, "Can't find first border (%d, %d) coordinates", (int)est_u, (int)est_v); + vpDEBUG_TRACE(3, "Can't find first border (%d, %d) coordinates", static_cast(est_u), static_cast(est_v)); return false; } @@ -1619,8 +1633,8 @@ bool vpDot2::computeParameters(const vpImage &I, const double &_u ip_edges_list.push_back(ip); - int border_u = (int)this->firstBorder_u; - int border_v = (int)this->firstBorder_v; + int border_u = static_cast(this->firstBorder_u); + int border_v = static_cast(this->firstBorder_v); int du, dv; float dS, dMu, dMv, dMuv, dMu2, dMv2; m00 = 0.0; @@ -1633,7 +1647,7 @@ bool vpDot2::computeParameters(const vpImage &I, const double &_u do { // if it was asked, show the border if (graphics) { - for (int t = 0; t < (int)thickness; t++) { + for (int t = 0; t < static_cast(thickness); ++t) { ip.set_u(border_u + t); ip.set_v(border_v); @@ -1663,7 +1677,7 @@ bool vpDot2::computeParameters(const vpImage &I, const double &_u m02 += dMv2; // Second order moment along u axis } // if we are now out of the image, return an error tracking - if (!isInArea((unsigned int)border_u, (unsigned int)border_v)) { + if (!isInArea(static_cast(border_u), static_cast(border_v))) { vpDEBUG_TRACE(3, "Dot (%d, %d) is not in the area", border_u, border_v); // Can Occur on a single pixel dot located on the top border @@ -1678,20 +1692,22 @@ bool vpDot2::computeParameters(const vpImage &I, const double &_u ip.set_v(border_v); ip_edges_list.push_back(ip); - // vpDisplay::getClick(I); - // update the extreme point of the dot. - if (border_v < bbox_v_min) + if (border_v < bbox_v_min) { bbox_v_min = border_v; - if (border_v > bbox_v_max) + } + if (border_v > bbox_v_max) { bbox_v_max = border_v; - if (border_u < bbox_u_min) + } + if (border_u < bbox_u_min) { bbox_u_min = border_u; - if (border_u > bbox_u_max) + } + if (border_u > bbox_u_max) { bbox_u_max = border_u; + } // move around the tracked entity by following the border. - if (computeFreemanChainElement(I, (unsigned int)border_u, (unsigned int)border_v, dir) == false) { + if (computeFreemanChainElement(I, static_cast(border_u), static_cast(border_v), dir) == false) { vpDEBUG_TRACE(3, "Can't compute Freeman chain for dot (%d, %d)", border_u, border_v); return false; } @@ -1699,9 +1715,9 @@ bool vpDot2::computeParameters(const vpImage &I, const double &_u // vpTRACE("border_u: %d border_v: %d dir: %d", border_u, border_v, // dir); - } while ((getFirstBorder_u() != (unsigned int)border_u || getFirstBorder_v() != (unsigned int)border_v || - firstDir != dir) && - isInArea((unsigned int)border_u, (unsigned int)border_v)); + } while (((getFirstBorder_u() != static_cast(border_u)) || (getFirstBorder_v() != static_cast(border_v)) || + (firstDir != dir)) && + isInArea(static_cast(border_u), static_cast(border_v))); #ifdef VP_DEBUG #if VP_DEBUG_MODE == 3 @@ -1712,8 +1728,8 @@ bool vpDot2::computeParameters(const vpImage &I, const double &_u // if the surface is one or zero , the center of gravity wasn't properly // detected. Return an error tracking. // if( m00 == 0 || m00 == 1 ) - if (std::fabs(m00) <= std::numeric_limits::epsilon() || - std::fabs(m00 - 1.) <= vpMath::maximum(std::fabs(m00), 1.) * std::numeric_limits::epsilon()) { + if ((std::fabs(m00) <= std::numeric_limits::epsilon()) || + (std::fabs(m00 - 1.) <= (vpMath::maximum(std::fabs(m00), 1.) * std::numeric_limits::epsilon()))) { vpDEBUG_TRACE(3, "The center of gravity of the dot wasn't properly detected"); return false; } @@ -1725,17 +1741,17 @@ bool vpDot2::computeParameters(const vpImage &I, const double &_u // Updates the second order centered moments if (compute_moment) { - mu11 = m11 - tmpCenter_u * m01; - mu02 = m02 - tmpCenter_v * m01; - mu20 = m20 - tmpCenter_u * m10; + mu11 = m11 - (tmpCenter_u * m01); + mu02 = m02 - (tmpCenter_v * m01); + mu20 = m20 - (tmpCenter_u * m10); } cog.set_u(tmpCenter_u); cog.set_v(tmpCenter_v); } - width = bbox_u_max - bbox_u_min + 1; - height = bbox_v_max - bbox_v_min + 1; + width = (bbox_u_max - bbox_u_min) + 1; + height = (bbox_v_max - bbox_v_min) + 1; surface = m00; computeMeanGrayLevel(I); @@ -1773,11 +1789,11 @@ bool vpDot2::findFirstBorder(const vpImage &I, const unsigned int #ifdef DEBUG std::cout << "gray level: " << gray_level_min << " " << gray_level_max << std::endl; #endif - while (hasGoodLevel(I, border_u + 1, border_v) && border_u < area.getRight() /*I.getWidth()*/) { + while (hasGoodLevel(I, border_u + 1, border_v) && (border_u < area.getRight()) /*I.getWidth()*/) { // if the width of this dot was initialised and we already crossed the dot // on more than the max possible width, no need to continue, return an // error tracking - if (getWidth() > 0 && (border_u - u) > getWidth() / (getMaxSizeSearchDistancePrecision() + epsilon)) { + if ((getWidth() > 0) && ((border_u - u) > (getWidth() / (getMaxSizeSearchDistancePrecision()) + epsilon))) { vpDEBUG_TRACE(3, "The found dot (%d, %d, %d) has a greater width than the " "required one", @@ -1817,67 +1833,67 @@ bool vpDot2::computeFreemanChainElement(const vpImage &I, const u { if (hasGoodLevel(I, u, v)) { - unsigned int _u = u; - unsigned int _v = v; + unsigned int v_u = u; + unsigned int v_v = v; // get the point on the right of the point passed in - updateFreemanPosition(_u, _v, (element + 2) % 8); - if (hasGoodLevel(I, _u, _v)) { + updateFreemanPosition(v_u, v_v, (element + 2) % 8); + if (hasGoodLevel(I, v_u, v_v)) { element = (element + 2) % 8; // turn right } else { - unsigned int _u1 = u; - unsigned int _v1 = v; - updateFreemanPosition(_u1, _v1, (element + 1) % 8); + unsigned int v_u1 = u; + unsigned int v_v1 = v; + updateFreemanPosition(v_u1, v_v1, (element + 1) % 8); - if (hasGoodLevel(I, _u1, _v1)) { + if (hasGoodLevel(I, v_u1, v_v1)) { element = (element + 1) % 8; // turn diag right } else { - unsigned int _u2 = u; - unsigned int _v2 = v; - updateFreemanPosition(_u2, _v2, element); // same direction + unsigned int v_u2 = u; + unsigned int v_v2 = v; + updateFreemanPosition(v_u2, v_v2, element); // same direction - if (hasGoodLevel(I, _u2, _v2)) { + if (hasGoodLevel(I, v_u2, v_v2)) { // element = element; // keep same dir } else { - unsigned int _u3 = u; - unsigned int _v3 = v; - updateFreemanPosition(_u3, _v3, (element + 7) % 8); // diag left + unsigned int v_u3 = u; + unsigned int v_v3 = v; + updateFreemanPosition(v_u3, v_v3, (element + 7) % 8); // diag left - if (hasGoodLevel(I, _u3, _v3)) { + if (hasGoodLevel(I, v_u3, v_v3)) { element = (element + 7) % 8; // turn diag left } else { - unsigned int _u4 = u; - unsigned int _v4 = v; - updateFreemanPosition(_u4, _v4, (element + 6) % 8); // left + unsigned int v_u4 = u; + unsigned int v_v4 = v; + updateFreemanPosition(v_u4, v_v4, (element + 6) % 8); // left - if (hasGoodLevel(I, _u4, _v4)) { + if (hasGoodLevel(I, v_u4, v_v4)) { element = (element + 6) % 8; // turn left } else { - unsigned int _u5 = u; - unsigned int _v5 = v; - updateFreemanPosition(_u5, _v5, (element + 5) % 8); // left + unsigned int v_u5 = u; + unsigned int v_v5 = v; + updateFreemanPosition(v_u5, v_v5, (element + 5) % 8); // left - if (hasGoodLevel(I, _u5, _v5)) { + if (hasGoodLevel(I, v_u5, v_v5)) { element = (element + 5) % 8; // turn diag down } else { - unsigned int _u6 = u; - unsigned int _v6 = v; - updateFreemanPosition(_u6, _v6, (element + 4) % 8); // left + unsigned int v_u6 = u; + unsigned int v_v6 = v; + updateFreemanPosition(v_u6, v_v6, (element + 4) % 8); // left - if (hasGoodLevel(I, _u6, _v6)) { + if (hasGoodLevel(I, v_u6, v_v6)) { element = (element + 4) % 8; // turn down } else { - unsigned int _u7 = u; - unsigned int _v7 = v; - updateFreemanPosition(_u7, _v7, (element + 3) % 8); // diag + unsigned int v_u7 = u; + unsigned int v_v7 = v; + updateFreemanPosition(v_u7, v_v7, (element + 3) % 8); // diag - if (hasGoodLevel(I, _u7, _v7)) { + if (hasGoodLevel(I, v_u7, v_v7)) { element = (element + 3) % 8; // turn diag right down } else { @@ -1951,38 +1967,38 @@ void vpDot2::computeFreemanParameters(const int &u_p, const int &v_p, unsigned i switch (element) { case 0: // go right du = 1; - dS = (float)v_p; + dS = static_cast(v_p); dMu = 0.0; - dMv = (float)(0.5 * v_p * v_p); + dMv = static_cast(0.5 * v_p * v_p); if (compute_moment) { - dMuv = (float)(0.25 * v_p * v_p * (2 * u_p + 1)); + dMuv = static_cast(0.25 * v_p * v_p * ((2 * u_p) + 1)); dMu2 = 0; - dMv2 = (float)(1.0 / 3. * v_p * v_p * v_p); + dMv2 = static_cast((1.0 / 3.) * v_p * v_p * v_p); } break; case 1: // go right top du = 1; dv = 1; - dS = (float)(v_p + 0.5); - dMu = -(float)(0.5 * u_p * (u_p + 1) + 1.0 / 6.0); - dMv = (float)(0.5 * v_p * (v_p + 1) + 1.0 / 6.0); + dS = static_cast(v_p + 0.5); + dMu = -static_cast((0.5 * u_p * (u_p + 1)) + (1.0 / 6.0)); + dMv = static_cast((0.5 * v_p * (v_p + 1)) + (1.0 / 6.0)); if (compute_moment) { - float half_u_p = (float)(0.5 * u_p); - dMuv = (float)(v_p * v_p * (0.25 + half_u_p) + v_p * (1. / 3. + half_u_p) + 1. / 6. * u_p + 0.125); - dMu2 = (float)(-1. / 3. * u_p * (u_p * u_p + 1.5 * u_p + 1.) - 1. / 12.0); - dMv2 = (float)(1. / 3. * v_p * (v_p * v_p + 1.5 * v_p + 1.) + 1. / 12.0); + float half_u_p = static_cast(0.5 * u_p); + dMuv = static_cast(v_p * v_p * (0.25 + half_u_p) + v_p * (1. / 3. + half_u_p) + 1. / 6. * u_p + 0.125); + dMu2 = static_cast((-1. / 3.) * u_p * ((u_p * u_p) + (1.5 * u_p) + 1.) - (1. / 12.0)); + dMv2 = static_cast((1. / 3.) * v_p * ((v_p * v_p) + (1.5 * v_p) + 1.) + (1. / 12.0)); } break; case 2: // go top dv = 1; dS = 0.0; - dMu = (float)(-0.5 * u_p * u_p); + dMu = static_cast(-0.5 * u_p * u_p); dMv = 0.0; if (compute_moment) { dMuv = 0; - dMu2 = (float)(-1.0 / 3. * u_p * u_p * u_p); + dMu2 = static_cast((-1.0 / 3.) * u_p * u_p * u_p); dMv2 = 0; } break; @@ -1990,51 +2006,51 @@ void vpDot2::computeFreemanParameters(const int &u_p, const int &v_p, unsigned i case 3: du = -1; dv = 1; - dS = (float)(-v_p - 0.5); - dMu = -(float)(0.5 * u_p * (u_p - 1) + 1.0 / 6.0); - dMv = -(float)(0.5 * v_p * (v_p + 1) + 1.0 / 6.0); + dS = static_cast(-v_p - 0.5); + dMu = -static_cast((0.5 * u_p * (u_p - 1)) + (1.0 / 6.0)); + dMv = -static_cast((0.5 * v_p * (v_p + 1)) + (1.0 / 6.0)); if (compute_moment) { - float half_u_p = (float)(0.5 * u_p); - dMuv = (float)(v_p * v_p * (0.25 - half_u_p) + v_p * (1. / 3. - half_u_p) - 1. / 6. * u_p + 0.125); - dMu2 = (float)(-1. / 3. * u_p * (u_p * u_p - 1.5 * u_p + 1.) - 1. / 12.0); - dMv2 = (float)(-1. / 3. * v_p * (v_p * v_p + 1.5 * v_p + 1.) - 1. / 12.0); + float half_u_p = static_cast(0.5 * u_p); + dMuv = static_cast(v_p * v_p * (0.25 - half_u_p) + (v_p * ((1. / 3.) - half_u_p)) - ((1. / 6.) * u_p) + 0.125); + dMu2 = static_cast(-1. / 3. * u_p * (u_p * u_p - 1.5 * u_p + 1.) - (1. / 12.0)); + dMv2 = static_cast(-1. / 3. * v_p * ((v_p * v_p) + (1.5 * v_p) + 1.) - (1. / 12.0)); } break; case 4: du = -1; - dS = (float)(-v_p); - dMv = (float)(-0.5 * v_p * v_p); + dS = static_cast(-v_p); + dMv = static_cast(-0.5 * v_p * v_p); dMu = 0.0; if (compute_moment) { - dMuv = (float)(-0.25 * v_p * v_p * (2 * u_p - 1)); + dMuv = static_cast(-0.25 * v_p * v_p * ((2 * u_p) - 1)); dMu2 = 0; - dMv2 = (float)(-1.0 / 3. * v_p * v_p * v_p); + dMv2 = static_cast((-1.0 / 3.) * v_p * v_p * v_p); } break; case 5: du = -1; dv = -1; - dS = (float)(-v_p + 0.5); - dMu = (float)(0.5 * u_p * (u_p - 1) + 1.0 / 6.0); - dMv = (float)(-(0.5 * v_p * (v_p - 1) + 1.0 / 6.0)); + dS = static_cast(-v_p + 0.5); + dMu = static_cast(0.5 * u_p * (u_p - 1) + (1.0 / 6.0)); + dMv = static_cast(-(0.5 * v_p * (v_p - 1) + (1.0 / 6.0))); if (compute_moment) { - float half_u_p = (float)(0.5 * u_p); - dMuv = (float)(v_p * v_p * (0.25 - half_u_p) - v_p * (1. / 3. - half_u_p) - 1. / 6. * u_p + 0.125); - dMu2 = (float)(1. / 3. * u_p * (u_p * u_p - 1.5 * u_p + 1.) - 1. / 12.0); - dMv2 = (float)(-1. / 3. * v_p * (v_p * v_p - 1.5 * v_p + 1.) - 1. / 12.0); + float half_u_p = static_cast(0.5 * u_p); + dMuv = static_cast(v_p * v_p * (0.25 - half_u_p) - (v_p * (1. / 3. - half_u_p)) - ((1. / 6.) * u_p) + 0.125); + dMu2 = static_cast((1. / 3.) * u_p * ((u_p * u_p) - (1.5 * u_p) + 1.) - (1. / 12.0)); + dMv2 = static_cast((-1. / 3.) * v_p * ((v_p * v_p) - (1.5 * v_p) + 1.) - (1. / 12.0)); } break; case 6: dv = -1; dS = 0.0; - dMu = (float)(0.5 * u_p * u_p); + dMu = static_cast(0.5 * u_p * u_p); dMv = 0.0; if (compute_moment) { dMuv = 0; - dMu2 = (float)(1.0 / 3. * u_p * u_p * u_p); + dMu2 = static_cast((1.0 / 3.) * u_p * u_p * u_p); dMv2 = 0; } break; @@ -2042,14 +2058,14 @@ void vpDot2::computeFreemanParameters(const int &u_p, const int &v_p, unsigned i case 7: du = 1; dv = -1; - dS = (float)(v_p - 0.5); - dMu = (float)(0.5 * u_p * (u_p + 1) + 1.0 / 6.0); - dMv = (float)(0.5 * v_p * (v_p - 1) + 1.0 / 6.0); + dS = static_cast(v_p - 0.5); + dMu = static_cast((0.5 * u_p * (u_p + 1)) + (1.0 / 6.0)); + dMv = static_cast((0.5 * v_p * (v_p - 1)) + (1.0 / 6.0)); if (compute_moment) { - float half_u_p = (float)(0.5 * u_p); - dMuv = (float)(v_p * v_p * (0.25 + half_u_p) - v_p * (1. / 3. + half_u_p) + 1. / 6. * u_p + 0.125); - dMu2 = (float)(1. / 3. * u_p * (u_p * u_p + 1.5 * u_p + 1.) + 1. / 12.0); - dMv2 = (float)(1. / 3. * v_p * (v_p * v_p - 1.5 * v_p + 1.) - 1. / 12.0); + float half_u_p = static_cast(0.5 * u_p); + dMuv = static_cast((v_p * v_p * (0.25 + half_u_p)) - (v_p * ((1. / 3.) + half_u_p)) + ((1. / 6.) * u_p) + 0.125); + dMu2 = static_cast((1. / 3.) * u_p * (u_p * u_p + 1.5 * u_p + 1.) + (1. / 12.0)); + dMv2 = static_cast((1. / 3.) * v_p * (v_p * v_p - 1.5 * v_p + 1.) - (1. / 12.0)); } break; } @@ -2099,6 +2115,8 @@ void vpDot2::updateFreemanPosition(unsigned int &u, unsigned int &v, const unsig u += 1; v -= 1; break; + default: + std::cout << "In vpDot2::updateFreemanPosition dir not identified" << std::endl; } } @@ -2133,10 +2151,12 @@ bool vpDot2::isInImage(const vpImage &I, const vpImagePoint &ip) double u = ip.get_u(); double v = ip.get_v(); - if (u < 0 || u >= w) + if ((u < 0) || (u >= w)) { return false; - if (v < 0 || v >= h) + } + if ((v < 0) || (v >= h)) { return false; + } return true; } @@ -2153,15 +2173,17 @@ bool vpDot2::isInImage(const vpImage &I, const vpImagePoint &ip) */ bool vpDot2::isInArea(const unsigned int &u, const unsigned int &v) const { - unsigned int area_u_min = (unsigned int)area.getLeft(); - unsigned int area_u_max = (unsigned int)area.getRight(); - unsigned int area_v_min = (unsigned int)area.getTop(); - unsigned int area_v_max = (unsigned int)area.getBottom(); + unsigned int area_u_min = static_cast(area.getLeft()); + unsigned int area_u_max = static_cast(area.getRight()); + unsigned int area_v_min = static_cast(area.getTop()); + unsigned int area_v_max = static_cast(area.getBottom()); - if (u < area_u_min || u > area_u_max) + if ((u < area_u_min) || (u > area_u_max)) { return false; - if (v < area_v_min || v > area_v_max) + } + if ((v < area_v_min) || (v > area_v_max)) { return false; + } return true; } @@ -2183,13 +2205,15 @@ void vpDot2::getGridSize(unsigned int &gridWidth, unsigned int &gridHeight) // contained in the dot. We gent this here if the dot is a perfect disc. // More accurate criterium to define the grid should be implemented if // necessary - gridWidth = (unsigned int)(getWidth() * getMaxSizeSearchDistancePrecision() / sqrt(2.)); - gridHeight = (unsigned int)(getHeight() * getMaxSizeSearchDistancePrecision() / sqrt(2.0)); + gridWidth = static_cast((getWidth() * getMaxSizeSearchDistancePrecision()) / sqrt(2.)); + gridHeight = static_cast((getHeight() * getMaxSizeSearchDistancePrecision()) / sqrt(2.0)); - if (gridWidth == 0) + if (gridWidth == 0) { gridWidth = 1; - if (gridHeight == 0) + } + if (gridHeight == 0) { gridHeight = 1; + } } /*! @@ -2206,22 +2230,22 @@ void vpDot2::getGridSize(unsigned int &gridWidth, unsigned int &gridHeight) */ void vpDot2::computeMeanGrayLevel(const vpImage &I) { - int cog_u = (int)cog.get_u(); - int cog_v = (int)cog.get_v(); + int cog_u = static_cast(cog.get_u()); + int cog_v = static_cast(cog.get_v()); unsigned int sum_value = 0; unsigned int nb_pixels = 0; - for (unsigned int i = (unsigned int)this->bbox_u_min; i <= (unsigned int)this->bbox_u_max; i++) { - unsigned int pixel_gray = (unsigned int)I[(unsigned int)cog_v][i]; + for (unsigned int i = static_cast(this->bbox_u_min); i <= static_cast(this->bbox_u_max); ++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++; } } - for (unsigned int i = (unsigned int)this->bbox_v_min; i <= (unsigned int)this->bbox_v_max; i++) { - unsigned char pixel_gray = I[i][(unsigned int)cog_u]; - if (pixel_gray >= getGrayLevelMin() && pixel_gray <= getGrayLevelMax()) { + 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++; } @@ -2241,9 +2265,9 @@ void vpDot2::computeMeanGrayLevel(const vpImage &I) else { imax = bbox_u_max - cog_u; } - for (int i = -imin; i <= imax; i++) { - unsigned int pixel_gray = (unsigned int)I[(unsigned int)(cog_v + i)][(unsigned int)(cog_u + i)]; - if (pixel_gray >= getGrayLevelMin() && pixel_gray <= getGrayLevelMax()) { + for (int i = -imin; i <= imax; ++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++; } @@ -2262,9 +2286,9 @@ void vpDot2::computeMeanGrayLevel(const vpImage &I) imax = bbox_u_max - cog_u; } - for (int i = -imin; i <= imax; i++) { - unsigned char pixel_gray = I[(unsigned int)(cog_v - i)][(unsigned int)(cog_u + i)]; - if (pixel_gray >= getGrayLevelMin() && pixel_gray <= getGrayLevelMax()) { + for (int i = -imin; i <= imax; ++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++; } @@ -2337,14 +2361,16 @@ vpMatrix vpDot2::defineDots(vpDot2 dot[], const unsigned int &n, const std::stri vpDisplay::flush(I); // check that dots are far away ones from the other - for (i = 0; i < n && fromFile; ++i) { + for (i = 0; ((i < n) && fromFile); ++i) { double d = sqrt(vpMath::sqr(dot[i].getHeight()) + vpMath::sqr(dot[i].getWidth())); - for (unsigned int j = 0; j < n && fromFile; ++j) - if (j != i) + for (unsigned int j = 0; ((j < n) && fromFile); ++j) { + if (j != i) { if (dot[i].getDistance(dot[j]) < d) { fromFile = false; std::cout << "Dots from file seem incoherent" << std::endl; } + } + } } } @@ -2353,7 +2379,7 @@ vpMatrix vpDot2::defineDots(vpDot2 dot[], const unsigned int &n, const std::stri vpDisplay::flush(I); std::cout << "Click on the " << n << " dots clockwise starting from upper/left dot..." << std::endl; - for (i = 0; i < n; i++) { + for (i = 0; i < n; ++i) { if (trackDot) { dot[i].setGraphics(true); dot[i].initTracking(I); @@ -2370,14 +2396,15 @@ vpMatrix vpDot2::defineDots(vpDot2 dot[], const unsigned int &n, const std::stri } } - if (!fromFile && (dotFile != "")) { + if ((!fromFile) && (dotFile != "")) { vpMatrix::saveMatrix(dotFile, Cogs); std::cout << Cogs.getRows() << " dots written to file " << dotFile << std::endl; } // back to non graphic mode - for (i = 0; i < n; ++i) + for (i = 0; i < n; ++i) { dot[i].setGraphics(false); + } return Cogs; } @@ -2408,11 +2435,14 @@ void vpDot2::trackAndDisplay(vpDot2 dot[], const unsigned int &n, vpImage &I, const vpImagePoint &cog, const std::list &edges_list, vpColor color, unsigned int thickness) { - vpDisplay::displayCross(I, cog, 3 * thickness + 8, color, thickness); + vpDisplay::displayCross(I, cog, (3 * thickness) + 8, color, thickness); std::list::const_iterator it; for (it = edges_list.begin(); it != edges_list.end(); ++it) { @@ -2465,7 +2495,7 @@ void vpDot2::display(const vpImage &I, const vpImagePoint &cog, void vpDot2::display(const vpImage &I, const vpImagePoint &cog, const std::list &edges_list, vpColor color, unsigned int thickness) { - vpDisplay::displayCross(I, cog, 3 * thickness + 8, color, thickness); + vpDisplay::displayCross(I, cog, (3 * thickness) + 8, color, thickness); std::list::const_iterator it; for (it = edges_list.begin(); it != edges_list.end(); ++it) { diff --git a/modules/vision/include/visp3/vision/vpHomography.h b/modules/vision/include/visp3/vision/vpHomography.h index 2f23e92eba..94f2c3f8b0 100644 --- a/modules/vision/include/visp3/vision/vpHomography.h +++ b/modules/vision/include/visp3/vision/vpHomography.h @@ -122,7 +122,7 @@ * // camera frame a * vpPoint Pa[4]; * std::vector xa(4), ya(4); // Coordinates in pixels of the points in frame a - * for(int i=0 ; i < 4 ; i++) { + * for(int i=0 ; i < 4 ; ++i) { * Pa[i] = Po[i]; Pa[i].project(aMo); // Project the points from object frame to camera frame a * vpMeterPixelConversion::convertPoint(cam, * Pa[i].get_x(), Pa[i].get_y(), @@ -133,7 +133,7 @@ * // camera frame b * vpPoint Pb[4]; * std::vector xb(4), yb(4); // Coordinates in pixels of the points in frame b - * for(int i=0 ; i < 4 ; i++) { + * for(int i=0 ; i < 4 ; ++i) { * Pb[i] = Po[i]; Pb[i].project(bMo); // Project the points from object frame to camera frame a * } * @@ -148,7 +148,7 @@ * // Compute the coordinates of the points in frame b using the ground * // truth homography and the coordinates of the points in frame a * vpHomography bHa = aHb.inverse(); - * for(int i = 0; i < 4 ; i++){ + * for(int i = 0; i < 4 ; ++i){ * double inv_z = 1. / (bHa[2][0] * xa[i] + bHa[2][1] * ya[i] + bHa[2][2]); * * xb[i] = (bHa[0][0] * xa[i] + bHa[0][1] * ya[i] + bHa[0][2]) * inv_z; diff --git a/modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.cpp b/modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.cpp index 2463d69225..f2c7743690 100644 --- a/modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.cpp +++ b/modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.cpp @@ -61,10 +61,10 @@ double enorm(const double *x, int n) double s1 = 0.0, s2 = 0.0, s3 = 0.0; double x1max = 0.0, x3max = 0.0; - floatn = (double)n; + floatn = static_cast(n); agiant = rgiant / floatn; - for (i = 0; i < n; i++) { + for (i = 0; i < n; ++i) { double xabs = std::fabs(x[i]); if ((xabs > rdwarf) && (xabs < agiant)) { /* @@ -78,12 +78,13 @@ double enorm(const double *x, int n) * somme pour elements petits. */ if (xabs <= x3max) { - // if (xabs != 0.0) - if (xabs > std::numeric_limits::epsilon()) + // --in comment: if (xabs != 0.0) + if (xabs > std::numeric_limits::epsilon()) { s3 += (xabs / x3max) * (xabs / x3max); + } } else { - s3 = 1.0 + s3 * (x3max / xabs) * (x3max / xabs); + s3 = 1.0 + (s3 * (x3max / xabs) * (x3max / xabs)); x3max = xabs; } } @@ -96,7 +97,7 @@ double enorm(const double *x, int n) s1 += (xabs / x1max) * (xabs / x1max); } else { - s1 = 1.0 + s1 * (x1max / xabs) * (x1max / xabs); + s1 = 1.0 + (s1 * (x1max / xabs) * (x1max / xabs)); x1max = xabs; } } @@ -105,20 +106,24 @@ double enorm(const double *x, int n) /* * calcul de la norme. */ - // if (s1 == 0.0) + // --in comment: if (s1 == 0.0) if (std::fabs(s1) <= std::numeric_limits::epsilon()) { - // if (s2 == 0.0) - if (std::fabs(s2) <= std::numeric_limits::epsilon()) + // --in comment: if (s2 == 0.0) + if (std::fabs(s2) <= std::numeric_limits::epsilon()) { norm_eucl = x3max * sqrt(s3); - else if (s2 >= x3max) - norm_eucl = sqrt(s2 * (1.0 + (x3max / s2) * (x3max * s3))); - else /*if (s2 < x3max)*/ + } + else if (s2 >= x3max) { + norm_eucl = sqrt(s2 * (1.0 + ((x3max / s2) * (x3max * s3)))); + } + else { /*if (s2 < x3max)*/ norm_eucl = sqrt(x3max * ((s2 / x3max) + (x3max * s3))); + } + } + else { + norm_eucl = x1max * sqrt(s1 + ((s2 / x1max) / x1max)); } - else - norm_eucl = x1max * sqrt(s1 + (s2 / x1max) / x1max); - return (norm_eucl); + return norm_eucl; } int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, double *delta, double *par, double *x, @@ -140,32 +145,34 @@ int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doubl */ nsing = n; - for (int i = 0; i < n; i++) { + for (int i = 0; i < n; ++i) { wa1[i] = qtb[i]; double *pt = MIJ(r, i, i, ldr); - // if (*MIJ(r, i, i, ldr) == 0.0 && nsing == n) - if (std::fabs(*pt) <= std::numeric_limits::epsilon() && nsing == n) + if ((std::fabs(*pt) <= std::numeric_limits::epsilon()) && (nsing == n)) { nsing = i - 1; + } - if (nsing < n) + if (nsing < n) { wa1[i] = 0.0; + } } if (nsing >= 0) { - for (int k = 0; k < nsing; k++) { + for (int k = 0; k < nsing; ++k) { int i = nsing - 1 - k; wa1[i] /= *MIJ(r, i, i, ldr); temp = wa1[i]; int jm1 = i - 1; if (jm1 >= 0) { - for (unsigned int j = 0; j <= (unsigned int)jm1; j++) + for (unsigned int j = 0; j <= static_cast(jm1); ++j) { wa1[j] -= *MIJ(r, i, j, ldr) * temp; + } } } } - for (int j = 0; j < n; j++) { + for (int j = 0; j < n; ++j) { l = ipvt[j]; x[l] = wa1[j]; } @@ -177,14 +184,15 @@ int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doubl */ iter = 0; - for (int i = 0; i < n; i++) + for (int i = 0; i < n; ++i) { wa2[i] = diag[i] * x[i]; + } dxnorm = enorm(wa2, n); fp = dxnorm - *delta; - if (fp > tol1 * (*delta)) { + if (fp > (tol1 * (*delta))) { /* * Si le jacobien n'a pas de rangee deficiente,l'etape de * Newton fournit une limite inferieure, parl pour le @@ -193,19 +201,20 @@ int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doubl double parl = 0.0; if (nsing >= n) { - for (int i = 0; i < n; i++) { + for (int i = 0; i < n; ++i) { l = ipvt[i]; wa1[i] = diag[l] * (wa2[l] / dxnorm); } - for (int i = 0; i < n; i++) { + for (int i = 0; i < n; ++i) { long im1; double sum = 0.0; im1 = (i - 1L); if (im1 >= 0) { - for (unsigned int j = 0; j <= (unsigned int)im1; j++) + for (unsigned int j = 0; j <= static_cast(im1); ++j) { sum += (*MIJ(r, i, j, ldr) * wa1[j]); + } } wa1[i] = (wa1[i] - sum) / *MIJ(r, i, i, ldr); } @@ -218,11 +227,12 @@ int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doubl * calcul d'une limite superieure, paru, pour le zero de la * fonction. */ - for (int i = 0; i < n; i++) { + for (int i = 0; i < n; ++i) { double sum = 0.0; - for (int j = 0; j <= i; j++) + for (int j = 0; j <= i; ++j) { sum += *MIJ(r, i, j, ldr) * qtb[j]; + } l = ipvt[i]; wa1[i] = sum / diag[l]; @@ -231,9 +241,10 @@ int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doubl double gnorm = enorm(wa1, n); double paru = gnorm / *delta; - // if (paru == 0.0) - if (std::fabs(paru) <= std::numeric_limits::epsilon()) + /* --in comment: if (paru == 0.0) */ + if (std::fabs(paru) <= std::numeric_limits::epsilon()) { paru = dwarf / vpMath::minimum(*delta, tol1); + } /* * Si "par" en entree tombe hors de l'intervalle (parl,paru), @@ -243,9 +254,9 @@ int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doubl *par = vpMath::maximum(*par, parl); *par = vpMath::maximum(*par, paru); - // if (*par == 0.0) - if (std::fabs(*par) <= std::numeric_limits::epsilon()) + if (std::fabs(*par) <= std::numeric_limits::epsilon()) { *par = gnorm / dxnorm; + } /* * debut d'une iteration. @@ -258,7 +269,6 @@ int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doubl * evaluation de la fonction a la valeur courant * de "par". */ - // if (*par == 0.0) if (std::fabs(*par) <= std::numeric_limits::epsilon()) { const double tol001 = 0.001; /* tolerance a 0.001 */ *par = vpMath::maximum(dwarf, (tol001 * paru)); @@ -266,13 +276,15 @@ int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doubl temp = sqrt(*par); - for (int i = 0; i < n; i++) + for (int i = 0; i < n; ++i) { wa1[i] = temp * diag[i]; + } qrsolv(n, r, ldr, ipvt, wa1, qtb, x, sdiag, wa2); - for (int i = 0; i < n; i++) + for (int i = 0; i < n; ++i) { wa2[i] = diag[i] * x[i]; + } dxnorm = enorm(wa2, n); temp = fp; @@ -284,37 +296,31 @@ 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)) || ((parl == 0.0) && (fp <= - // temp) - // && (temp < 0.0)) || (iter == 10)) - if ((std::fabs(fp) <= tol1 * (*delta)) || - ((std::fabs(parl) <= std::numeric_limits::epsilon()) && (fp <= temp) && (temp < 0.0)) || + if ((std::fabs(fp) <= (tol1 * (*delta))) || + ((std::fabs(parl) <= std::numeric_limits::epsilon()) && ((fp <= temp) && (temp < 0.0))) || (iter == 10)) { // terminaison. - // Remove the two next lines since this is a dead code - /* if (iter == 0) - *par = 0.0; */ - - return (0); + return 0; } /* * calcul de la correction de Newton. */ - for (int i = 0; i < n; i++) { + for (int i = 0; i < n; ++i) { l = ipvt[i]; wa1[i] = diag[l] * (wa2[l] / dxnorm); } - for (unsigned int i = 0; i < (unsigned int)n; i++) { + for (unsigned int i = 0; i < static_cast(n); ++i) { wa1[i] = wa1[i] / sdiag[i]; temp = wa1[i]; unsigned int jp1 = i + 1; - if ((unsigned int)n >= jp1) { - for (unsigned int j = jp1; j < (unsigned int)n; j++) + if (static_cast(n) >= jp1) { + for (unsigned int j = jp1; j < static_cast(n); ++j) { wa1[j] -= (*MIJ(r, i, j, ldr) * temp); + } } } @@ -325,11 +331,13 @@ int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doubl * selon le signe de la fonction, mise a jour * de parl ou paru. */ - if (fp > 0.0) + if (fp > 0.0) { parl = vpMath::maximum(parl, *par); + } - if (fp < 0.0) + if (fp < 0.0) { paru = vpMath::minimum(paru, *par); + } /* * calcul d'une estimee ameliree de "par". @@ -341,10 +349,11 @@ int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doubl /* * terminaison. */ - if (iter == 0) + if (iter == 0) { *par = 0.0; + } - return (0); + return 0; } double pythag(double a, double b) @@ -353,26 +362,26 @@ double pythag(double a, double b) p = vpMath::maximum(std::fabs(a), std::fabs(b)); - // if (p == 0.0) + /* --in comment: if (p == 0.0) */ if (std::fabs(p) <= std::numeric_limits::epsilon()) { pyth = p; - return (pyth); + return pyth; } r = (std::min(std::fabs(a), std::fabs(b)) / p) * (std::min(std::fabs(a), std::fabs(b)) / p); t = 4.0 + r; - // while (t != 4.0) - while (std::fabs(t - 4.0) < std::fabs(vpMath::maximum(t, 4.0)) * std::numeric_limits::epsilon()) { + /* --in comment: while (t != 4.0) */ + while (std::fabs(t - 4.0) < (std::fabs(vpMath::maximum(t, 4.0)) * std::numeric_limits::epsilon())) { double s = r / t; - double u = 1.0 + 2.0 * s; + double u = 1.0 + (2.0 * s); p *= u; r *= (s / u) * (s / u); t = 4.0 + r; } pyth = p; - return (pyth); + return pyth; } int qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt, int /* lipvt */, double *rdiag, double *acnorm, @@ -393,33 +402,36 @@ int qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt, int /* lipvt * calcul des normes initiales des lignes et initialisation * de plusieurs tableaux. */ - for (i = 0; i < m; i++) { + for (i = 0; i < m; ++i) { acnorm[i] = enorm(MIJ(a, i, 0, lda), n); rdiag[i] = acnorm[i]; wa[i] = rdiag[i]; - if (pivot) + if (pivot) { ipvt[i] = i; + } } /* * reduction de "a" en "r" avec les transformations de Householder. */ minmn = vpMath::minimum(m, n); - for (i = 0; i < minmn; i++) { + for (i = 0; i < minmn; ++i) { if (pivot) { /* * met la ligne de plus grande norme en position * de pivot. */ kmax = i; - for (k = i; k < m; k++) { - if (rdiag[k] > rdiag[kmax]) + for (k = i; k < m; ++k) { + if (rdiag[k] > rdiag[kmax]) { kmax = k; + } } if (kmax != i) { - for (j = 0; j < n; j++) + for (j = 0; j < n; ++j) { SWAP(*MIJ(a, i, j, lda), *MIJ(a, kmax, j, lda), tmp); + } rdiag[kmax] = rdiag[i]; wa[kmax] = wa[i]; @@ -434,13 +446,15 @@ int qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt, int /* lipvt */ double ajnorm = enorm(MIJ(a, i, i, lda), n - i); - // if (ajnorm != 0.0) + /* --in comment: if (ajnorm != 0.0) */ if (std::fabs(ajnorm) > std::numeric_limits::epsilon()) { - if (*MIJ(a, i, i, lda) < 0.0) + if (*MIJ(a, i, i, lda) < 0.0) { ajnorm = -ajnorm; + } - for (j = i; j < n; j++) + for (j = i; j < n; ++j) { *MIJ(a, i, j, lda) /= ajnorm; + } *MIJ(a, i, i, lda) += 1.0; /* @@ -450,23 +464,24 @@ int qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt, int /* lipvt ip1 = i + 1; if (m >= ip1) { - for (k = ip1; k < m; k++) { + for (k = ip1; k < m; ++k) { sum = 0.0; - for (j = i; j < n; j++) + for (j = i; j < n; ++j) { sum += *MIJ(a, i, j, lda) * *MIJ(a, k, j, lda); + } temp = sum / *MIJ(a, i, i, lda); - for (j = i; j < n; j++) + for (j = i; j < n; ++j) { *MIJ(a, k, j, lda) -= temp * *MIJ(a, i, j, lda); + } - // if (pivot && rdiag[k] != 0.0) if (pivot && (std::fabs(rdiag[k]) > std::numeric_limits::epsilon())) { temp = *MIJ(a, k, i, lda) / rdiag[k]; rdiag[k] *= sqrt(vpMath::maximum(0.0, (1.0 - temp * temp))); - if (tolerance * (rdiag[k] / wa[k]) * (rdiag[k] / wa[k]) <= epsmch) { - rdiag[k] = enorm(MIJ(a, k, ip1, lda), (n - 1 - (int)i)); + if ((tolerance * (rdiag[k] / wa[k]) * (rdiag[k] / wa[k])) <= epsmch) { + rdiag[k] = enorm(MIJ(a, k, ip1, lda), (n - 1 - static_cast(i))); wa[k] = rdiag[k]; } } @@ -477,7 +492,7 @@ int qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt, int /* lipvt rdiag[i] = -ajnorm; } /* fin for (i = 0; i < minmn; i++) */ - return (0); + return 0; } int qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, double *x, double *sdiag, double *wa) @@ -491,9 +506,10 @@ int qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doub * et initialisation de "s". En particulier, sauvegarde des elements * diagonaux de "r" dans "x". */ - for (i = 0; i < n; i++) { - for (j = i; j < n; j++) + for (i = 0; i < n; ++i) { + for (j = i; j < n; ++j) { *MIJ(r, i, j, ldr) = *MIJ(r, j, i, ldr); + } x[i] = *MIJ(r, i, i, ldr); wa[i] = qtb[i]; @@ -504,7 +520,7 @@ int qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doub * connue. */ - for (i = 0; i < n; i++) { + for (i = 0; i < n; ++i) { /* * preparation de la colonne de d a eliminer, reperage de * l'element diagonal par utilisation de p de la @@ -512,10 +528,10 @@ int qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doub */ l = ipvt[i]; - // if (diag[l] != 0.0) if (std::fabs(diag[l]) > std::numeric_limits::epsilon()) { - for (k = i; k < n; k++) + for (k = i; k < n; ++k) { sdiag[k] = 0.0; + } sdiag[i] = diag[l]; @@ -528,23 +544,22 @@ int qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doub qtbpj = 0.0; - for (k = i; k < n; k++) { + for (k = i; k < n; ++k) { /* * determination d'une rotation qui elimine * les elements appropriees dans la colonne * courante de d. */ - // if (sdiag[k] != 0.0) if (std::fabs(sdiag[k]) > std::numeric_limits::epsilon()) { if (std::fabs(*MIJ(r, k, k, ldr)) >= std::fabs(sdiag[k])) { tg = sdiag[k] / *MIJ(r, k, k, ldr); - cosi = 0.5 / sqrt(0.25 + 0.25 * (tg * tg)); + cosi = 0.5 / sqrt(0.25 + (0.25 * (tg * tg))); sinu = cosi * tg; } else { cotg = *MIJ(r, k, k, ldr) / sdiag[k]; - sinu = 0.5 / sqrt(0.25 + 0.25 * (cotg * cotg)); + sinu = 0.5 / sqrt(0.25 + (0.25 * (cotg * cotg))); cosi = sinu * cotg; } @@ -553,9 +568,9 @@ int qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doub * de r et des elements modifies de * ((q transpose)*b,0). */ - *MIJ(r, k, k, ldr) = cosi * *MIJ(r, k, k, ldr) + sinu * sdiag[k]; - temp = cosi * wa[k] + sinu * qtbpj; - qtbpj = -sinu * wa[k] + cosi * qtbpj; + *MIJ(r, k, k, ldr) = (cosi * *MIJ(r, k, k, ldr)) + (sinu * sdiag[k]); + temp = (cosi * wa[k]) + (sinu * qtbpj); + qtbpj = (-sinu * wa[k]) + (cosi * qtbpj); wa[k] = temp; /* @@ -566,9 +581,9 @@ int qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doub kp1 = k + 1; if (n >= kp1) { - for (j = kp1; j < n; j++) { - temp = cosi * *MIJ(r, k, j, ldr) + sinu * sdiag[j]; - sdiag[j] = -sinu * *MIJ(r, k, j, ldr) + cosi * sdiag[j]; + for (j = kp1; j < n; ++j) { + temp = (cosi * *MIJ(r, k, j, ldr)) + (sinu * sdiag[j]); + sdiag[j] = (-sinu * *MIJ(r, k, j, ldr)) + (cosi * sdiag[j]); *MIJ(r, k, j, ldr) = temp; } } @@ -590,24 +605,26 @@ int qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doub */ nsing = n; - for (i = 0; i < n; i++) { - // if (sdiag[i] == 0.0 && nsing == n) - if ((std::fabs(sdiag[i]) <= std::numeric_limits::epsilon()) && nsing == n) + for (i = 0; i < n; ++i) { + if ((std::fabs(sdiag[i]) <= std::numeric_limits::epsilon()) && (nsing == n)) { nsing = i - 1; + } - if (nsing < n) + if (nsing < n) { wa[i] = 0.0; + } } if (nsing >= 0) { - for (k = 0; k < nsing; k++) { + for (k = 0; k < nsing; ++k) { i = nsing - 1 - k; double sum = 0.0; int jp1 = i + 1; if (nsing >= jp1) { - for (j = jp1; j < nsing; j++) + for (j = jp1; j < nsing; ++j) { sum += *MIJ(r, i, j, ldr) * wa[j]; + } } wa[i] = (wa[i] - sum) / sdiag[i]; } @@ -616,11 +633,11 @@ int qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doub * permutation arriere des composants de z et des componants de x. */ - for (j = 0; j < n; j++) { + for (j = 0; j < n; ++j) { l = ipvt[j]; x[l] = wa[j]; } - return (0); + return 0; } int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int iflag), int m, int n, @@ -647,54 +664,62 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, *njev = 0; /* verification des parametres d'entree. */ - - /*if (n <= 0) - return 0;*/ - if (m < n) + if (m < n) { return 0; - if (ldfjac < m) + } + if (ldfjac < m) { return 0; - if (ftol < 0.0) + } + if (ftol < 0.0) { return 0; - if (xtol < 0.0) + } + if (xtol < 0.0) { return 0; - if (gtol < 0.0) + } + if (gtol < 0.0) { return 0; - if (maxfev == 0) + } + if (maxfev == 0) { return 0; - if (factor <= 0.0) + } + 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)) { /* * termination, normal ou imposee par l'utilisateur. */ - if (iflag < 0) + if (iflag < 0) { *info = iflag; + } iflag = 0; - if (nprint > 0) + if (nprint > 0) { (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag); + } - return (count); + return count; } if (mode == 2) { - for (j = 0; j < n; j++) { + for (j = 0; j < n; ++j) { if (diag[j] <= 0.0) { /* * termination, normal ou imposee par l'utilisateur. */ - if (iflag < 0) + if (iflag < 0) { *info = iflag; + } iflag = 0; - if (nprint > 0) + if (nprint > 0) { (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag); + } - return (count); + return count; } } } @@ -713,15 +738,17 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, /* * termination, normal ou imposee par l'utilisateur. */ - if (iflag < 0) + if (iflag < 0) { *info = iflag; + } iflag = 0; - if (nprint > 0) + if (nprint > 0) { (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag); + } - return (count); + return count; } fnorm = enorm(fvec, m); @@ -737,7 +764,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 < (int)maxfev) { + while (count < static_cast(maxfev)) { count++; /* * calcul de la matrice jacobienne. @@ -753,15 +780,17 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, /* * termination, normal ou imposee par l'utilisateur. */ - if (iflag < 0) + if (iflag < 0) { *info = iflag; + } iflag = 0; - if (nprint > 0) + if (nprint > 0) { (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag); + } - return (count); + return count; } /* @@ -769,22 +798,25 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, */ if (nprint > 0) { iflag = 0; - if ((iter - 1) % nprint == 0) + if (((iter - 1) % nprint) == 0) { (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag); + } if (iflag < 0) { /* * termination, normal ou imposee par l'utilisateur. */ - if (iflag < 0) + if (iflag < 0) { *info = iflag; + } iflag = 0; - if (nprint > 0) + if (nprint > 0) { (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag); + } - return (count); + return count; } } @@ -800,11 +832,11 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, if (iter == 1) { if (mode != 2) { - for (j = 0; j < n; j++) { + for (j = 0; j < n; ++j) { diag[j] = wa2[j]; - // if (wa2[j] == 0.0) - if (std::fabs(wa2[j]) <= std::numeric_limits::epsilon()) + if (std::fabs(wa2[j]) <= std::numeric_limits::epsilon()) { diag[j] = 1.0; + } } } @@ -814,37 +846,40 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, * l'etape. */ - for (j = 0; j < n; j++) + for (j = 0; j < n; ++j) { wa3[j] = diag[j] * x[j]; + } xnorm = enorm(wa3, n); delta = factor * xnorm; - // if (delta == 0.0) - if (std::fabs(delta) <= std::numeric_limits::epsilon()) + if (std::fabs(delta) <= std::numeric_limits::epsilon()) { delta = factor; + } } /* * formation de (q transpose) * fvec et stockage des n premiers * composants dans qtf. */ - for (i = 0; i < m; i++) + for (i = 0; i < m; ++i) { wa4[i] = fvec[i]; + } - for (i = 0; i < n; i++) { + for (i = 0; i < n; ++i) { double *pt = MIJ(fjac, i, i, ldfjac); - // if (*MIJ(fjac, i, i, ldfjac) != 0.0) if (std::fabs(*pt) > std::numeric_limits::epsilon()) { sum = 0.0; - for (j = i; j < m; j++) + for (j = i; j < m; ++j) { sum += *MIJ(fjac, i, j, ldfjac) * wa4[j]; + } temp = -sum / *MIJ(fjac, i, i, ldfjac); - for (j = i; j < m; j++) + for (j = i; j < m; ++j) { wa4[j] += *MIJ(fjac, i, j, ldfjac) * temp; + } } *MIJ(fjac, i, i, ldfjac) = wa1[i]; @@ -857,15 +892,14 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, double gnorm = 0.0; - // if (fnorm != 0.0) if (std::fabs(fnorm) > std::numeric_limits::epsilon()) { - for (i = 0; i < n; i++) { + for (i = 0; i < n; ++i) { l = ipvt[i]; - // if (wa2[l] != 0.0) if (std::fabs(wa2[l]) > std::numeric_limits::epsilon()) { sum = 0.0; - for (j = 0; j <= i; j++) + for (j = 0; j <= i; ++j) { sum += *MIJ(fjac, i, j, ldfjac) * (qtf[j] / fnorm); + } gnorm = vpMath::maximum(gnorm, std::fabs(sum / wa2[l])); } @@ -876,22 +910,25 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, * test pour la convergence de la norme du gradient . */ - if (gnorm <= gtol) + if (gnorm <= gtol) { *info = 4; + } if (*info != 0) { /* * termination, normal ou imposee par l'utilisateur. */ - if (iflag < 0) + if (iflag < 0) { *info = iflag; + } iflag = 0; - if (nprint > 0) + if (nprint > 0) { (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag); + } - return (count); + return count; } /* @@ -899,8 +936,9 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, */ if (mode != 2) { - for (j = 0; j < n; j++) + for (j = 0; j < n; ++j) { diag[j] = vpMath::maximum(diag[j], wa2[j]); + } } /* @@ -918,7 +956,7 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, * stockage de la direction p et x + p. calcul de la norme de p. */ - for (j = 0; j < n; j++) { + for (j = 0; j < n; ++j) { wa1[j] = -wa1[j]; wa2[j] = x[j] + wa1[j]; wa3[j] = diag[j] * wa1[j]; @@ -931,8 +969,9 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, * l'etape. */ - if (iter == 1) + if (iter == 1) { delta = vpMath::minimum(delta, pnorm); + } /* * evaluation de la fonction en x + p et calcul de leur norme. @@ -945,15 +984,17 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, if (iflag < 0) { // termination, normal ou imposee par l'utilisateur. - if (iflag < 0) + if (iflag < 0) { *info = iflag; + } iflag = 0; - if (nprint > 0) + if (nprint > 0) { (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag); + } - return (count); + return count; } fnorm1 = enorm(wa4, m); @@ -964,20 +1005,22 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, actred = -1.0; - if ((tol1 * fnorm1) < fnorm) + if ((tol1 * fnorm1) < fnorm) { actred = 1.0 - ((fnorm1 / fnorm) * (fnorm1 / fnorm)); + } /* * calcul de la reduction predite mise a l'echelle et * de la derivee directionnelle mise a l'echelle. */ - for (i = 0; i < n; i++) { + for (i = 0; i < n; ++i) { wa3[i] = 0.0; l = ipvt[i]; temp = wa1[l]; - for (j = 0; j <= i; j++) + for (j = 0; j <= i; ++j) { wa3[j] += *MIJ(fjac, i, j, ldfjac) * temp; + } } temp1 = enorm(wa3, n) / fnorm; @@ -991,9 +1034,10 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, ratio = 0.0; - // if (prered != 0.0) - if (std::fabs(prered) > std::numeric_limits::epsilon()) + // --in comment: if (prered != 0.0) + if (std::fabs(prered) > std::numeric_limits::epsilon()) { ratio = actred / prered; + } /* * mise a jour de la limite de l'etape. @@ -1007,14 +1051,16 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, } } else { - if (actred >= 0.0) + if (actred >= 0.0) { temp = tol5; - - else + } + else { temp = tol5 * dirder / (dirder + tol5 * actred); + } - if ((tol1 * fnorm1 >= fnorm) || (temp < tol1)) + if ((tol1 * fnorm1 >= fnorm) || (temp < tol1)) { temp = tol1; + } delta = temp * vpMath::minimum(delta, (pnorm / tol1)); par /= temp; @@ -1029,13 +1075,14 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, * leurs normes. */ - for (j = 0; j < n; j++) { + for (j = 0; j < n; ++j) { x[j] = wa2[j]; wa2[j] = diag[j] * x[j]; } - for (i = 0; i < m; i++) + for (i = 0; i < m; ++i) { fvec[i] = wa4[i]; + } xnorm = enorm(wa2, n); fnorm = fnorm1; @@ -1046,59 +1093,70 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, * tests pour convergence. */ - if ((std::fabs(actred) <= ftol) && (prered <= ftol) && (tol5 * ratio <= 1.0)) + if ((std::fabs(actred) <= ftol) && (prered <= ftol) && (tol5 * ratio <= 1.0)) { *info = 1; + } - if (delta <= xtol * xnorm) + if (delta <= xtol * xnorm) { *info = 2; + } - if ((std::fabs(actred) <= ftol) && (prered <= ftol) && (tol5 * ratio <= 1.0) && *info == 2) + if ((std::fabs(actred) <= ftol) && (prered <= ftol) && (tol5 * ratio <= 1.0) && *info == 2) { *info = 3; + } if (*info != 0) { /* * termination, normal ou imposee par l'utilisateur. */ - if (iflag < 0) + if (iflag < 0) { *info = iflag; + } iflag = 0; - if (nprint > 0) + if (nprint > 0) { (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag); + } - return (count); + return count; } /* * tests pour termination et * verification des tolerances. */ - if (*nfev >= maxfev) + if (*nfev >= maxfev) { *info = 5; + } - if ((std::fabs(actred) <= epsmch) && (prered <= epsmch) && (tol5 * ratio <= 1.0)) + if ((std::fabs(actred) <= epsmch) && (prered <= epsmch) && (tol5 * ratio <= 1.0)) { *info = 6; + } - if (delta <= epsmch * xnorm) + if (delta <= epsmch * xnorm) { *info = 7; + } - if (gnorm <= epsmch) + if (gnorm <= epsmch) { *info = 8; + } if (*info != 0) { /* * termination, normal ou imposee par l'utilisateur. */ - if (iflag < 0) + if (iflag < 0) { *info = iflag; + } iflag = 0; - if (nprint > 0) + if (nprint > 0) { (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag); + } - return (count); + return count; } } /* fin while ratio >=tol0001 */ } /*fin while 1*/ @@ -1135,10 +1193,11 @@ int lmder1(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, lmder(ptr_fcn, m, n, x, fvec, fjac, ldfjac, ftol, xtol, gtol, maxfev, wa, mode, factor, nprint, info, &nfev, &njev, ipvt, &wa[n], &wa[2 * n], &wa[3 * n], &wa[4 * n], &wa[5 * n]); - if (*info == 8) + if (*info == 8) { *info = 4; + } - return (0); + return 0; } #undef TRUE diff --git a/modules/vision/src/pose-estimation/vpPose.cpp b/modules/vision/src/pose-estimation/vpPose.cpp index b81f76f9bd..08f34cb4b0 100644 --- a/modules/vision/src/pose-estimation/vpPose.cpp +++ b/modules/vision/src/pose-estimation/vpPose.cpp @@ -607,8 +607,8 @@ double vpPose::poseFromRectangle(vpPoint &p1, vpPoint &p2, vpPoint &p3, vpPoint // vpHomography::HartleyDLT(rectx,recty,irectx,irecty,hom); vpHomography::HLM(rectx, recty, irectx, irecty, 1, hom); - 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) H[i][j] = hom[i][j]; // calcul de s = ||Kh1||/ ||Kh2|| =ratio (length on x axis/ length on y // axis) diff --git a/modules/vision/src/pose-estimation/vpPoseDementhon.cpp b/modules/vision/src/pose-estimation/vpPoseDementhon.cpp index 603b1b156d..9d3a8f44a1 100644 --- a/modules/vision/src/pose-estimation/vpPoseDementhon.cpp +++ b/modules/vision/src/pose-estimation/vpPoseDementhon.cpp @@ -63,7 +63,7 @@ static void calculSolutionDementhon(vpColVector &I4, vpColVector &J4, vpHomogene double Z0 = 2.0 / (normI3 + normJ3); vpColVector I3(3), J3(3), K3(3); - for (unsigned int i = 0; i < 3; i++) { + for (unsigned int i = 0; i < 3; ++i) { I3[i] = I4[i] / normI3; J3[i] = J4[i] / normJ3; } @@ -106,7 +106,7 @@ void vpPose::poseDementhonNonPlan(vpHomogeneousMatrix &cMo) cdg[1] += P.get_oY(); cdg[2] += P.get_oZ(); } - for (unsigned int i = 0; i < 3; i++) + for (unsigned int i = 0; i < 3; ++i) cdg[i] /= npt; // printf("cdg : %lf %lf %lf\n", cdg[0], cdg[1],cdg[2]); @@ -122,7 +122,7 @@ void vpPose::poseDementhonNonPlan(vpHomogeneousMatrix &cMo) vpMatrix A(npt, 4), Ap; - for (unsigned int i = 0; i < npt; i++) { + for (unsigned int i = 0; i < npt; ++i) { A[i][0] = c3d[i].get_oX(); A[i][1] = c3d[i].get_oY(); A[i][2] = c3d[i].get_oZ(); @@ -140,7 +140,7 @@ void vpPose::poseDementhonNonPlan(vpHomogeneousMatrix &cMo) vpColVector xprim(npt); vpColVector yprim(npt); - for (unsigned int i = 0; i < npt; i++) { + for (unsigned int i = 0; i < npt; ++i) { xprim[i] = c3d[i].get_x(); yprim[i] = c3d[i].get_y(); } @@ -152,7 +152,7 @@ void vpPose::poseDementhonNonPlan(vpHomogeneousMatrix &cMo) calculSolutionDementhon(I4, J4, cMo); int erreur = 0; - for (unsigned int i = 0; i < npt; i++) { + for (unsigned int i = 0; i < npt; ++i) { double z; z = cMo[2][0] * c3d[i].get_oX() + cMo[2][1] * c3d[i].get_oY() + cMo[2][2] * c3d[i].get_oZ() + cMo[2][3]; if (z <= 0.0) @@ -174,7 +174,7 @@ void vpPose::poseDementhonNonPlan(vpHomogeneousMatrix &cMo) res_old = res; cMo_old = cMo; - for (unsigned int i = 0; i < npt; i++) { + for (unsigned int i = 0; i < npt; ++i) { double eps = (cMo[2][0] * c3d[i].get_oX() + cMo[2][1] * c3d[i].get_oY() + cMo[2][2] * c3d[i].get_oZ()) / cMo[2][3]; @@ -186,7 +186,7 @@ void vpPose::poseDementhonNonPlan(vpHomogeneousMatrix &cMo) calculSolutionDementhon(I4, J4, cMo); res = sqrt(computeResidualDementhon(cMo) / npt); - for (unsigned int i = 0; i < npt; i++) { + for (unsigned int i = 0; i < npt; ++i) { double z; z = cMo[2][0] * c3d[i].get_oX() + cMo[2][1] * c3d[i].get_oY() + cMo[2][2] * c3d[i].get_oZ() + cMo[2][3]; if (z <= 0.0) @@ -247,7 +247,7 @@ static void calculTwoSolutionsDementhonPlan(vpColVector &I04, vpColVector &J04, vpHomogeneousMatrix &cMo1, vpHomogeneousMatrix &cMo2) { vpColVector I0(3), J0(3); - for (unsigned int i = 0; i < 3; i++) { + for (unsigned int i = 0; i < 3; ++i) { I0[i] = I04[i]; J0[i] = J04[i]; } @@ -295,7 +295,7 @@ int vpPose::calculArbreDementhon(vpMatrix &Ap, vpColVector &U, vpHomogeneousMatr int erreur = 0; // test if all points are in front of the camera - for (unsigned int i = 0; i < npt; i++) { + for (unsigned int i = 0; i < npt; ++i) { double z; z = cMo[2][0] * c3d[i].get_oX() + cMo[2][1] * c3d[i].get_oY() + cMo[2][2] * c3d[i].get_oZ() + cMo[2][3]; if (z <= 0.0) { @@ -316,7 +316,7 @@ int vpPose::calculArbreDementhon(vpMatrix &Ap, vpColVector &U, vpHomogeneousMatr cMo_old = cMo; vpColVector xprim(npt), yprim(npt); - for (unsigned int i = 0; i < npt; i++) { + for (unsigned int i = 0; i < npt; ++i) { double eps = (cMo[2][0] * c3d[i].get_oX() + cMo[2][1] * c3d[i].get_oY() + cMo[2][2] * c3d[i].get_oZ()) / cMo[2][3]; @@ -333,7 +333,7 @@ int vpPose::calculArbreDementhon(vpMatrix &Ap, vpColVector &U, vpHomogeneousMatr // test if all points are in front of the camera for cMo1 and cMo2 int erreur1 = 0; int erreur2 = 0; - for (unsigned int i = 0; i < npt; i++) { + for (unsigned int i = 0; i < npt; ++i) { double z; z = cMo1[2][0] * c3d[i].get_oX() + cMo1[2][1] * c3d[i].get_oY() + cMo1[2][2] * c3d[i].get_oZ() + cMo1[2][3]; if (z <= 0.0) @@ -428,7 +428,7 @@ void vpPose::poseDementhonPlan(vpHomogeneousMatrix &cMo) cdg[1] += P.get_oY(); cdg[2] += P.get_oZ(); } - for (unsigned int i = 0; i < 3; i++) + for (unsigned int i = 0; i < 3; ++i) cdg[i] /= npt; // printf("cdg : %lf %lf %lf\n", cdg[0], cdg[1],cdg[2]); @@ -444,7 +444,7 @@ void vpPose::poseDementhonPlan(vpHomogeneousMatrix &cMo) vpMatrix A(npt, 4); - for (unsigned int i = 0; i < npt; i++) { + for (unsigned int i = 0; i < npt; ++i) { A[i][0] = c3d[i].get_oX(); A[i][1] = c3d[i].get_oY(); A[i][2] = c3d[i].get_oZ(); @@ -459,7 +459,7 @@ void vpPose::poseDementhonPlan(vpHomogeneousMatrix &cMo) int nbMaxIter = static_cast(std::max(std::ceil(logNOfSvdThresholdLimit - logNofSvdThresh), 1.)); double svdThreshold = m_dementhonSvThresh; int irank = 0; - for (int i = 0; i < nbMaxIter && !isRankEqualTo3; i++) { + for (int i = 0; i < nbMaxIter && !isRankEqualTo3; ++i) { irank = A.pseudoInverse(Ap, sv, svdThreshold, imA, imAt, kAt); if (irank == 3) { isRankEqualTo3 = true; @@ -483,7 +483,7 @@ void vpPose::poseDementhonPlan(vpHomogeneousMatrix &cMo) } // calcul de U vpColVector U(4); - for (unsigned int i = 0; i < 4; i++) { + for (unsigned int i = 0; i < 4; ++i) { U[i] = kAt[0][i]; } #if (DEBUG_LEVEL2) @@ -497,7 +497,7 @@ void vpPose::poseDementhonPlan(vpHomogeneousMatrix &cMo) vpColVector xi(npt); vpColVector yi(npt); - for (unsigned int i = 0; i < npt; i++) { + for (unsigned int i = 0; i < npt; ++i) { xi[i] = c3d[i].get_x(); yi[i] = c3d[i].get_y(); } @@ -570,7 +570,7 @@ double vpPose::computeResidualDementhon(const vpHomogeneousMatrix &cMo) // cMo here corresponds to object frame at that point, i.e, only the one used // internally to Dementhon's method - for (unsigned int i = 0; i < npt; i++) { + for (unsigned int i = 0; i < npt; ++i) { double X = c3d[i].get_oX() * cMo[0][0] + c3d[i].get_oY() * cMo[0][1] + c3d[i].get_oZ() * cMo[0][2] + cMo[0][3]; double Y = c3d[i].get_oX() * cMo[1][0] + c3d[i].get_oY() * cMo[1][1] + c3d[i].get_oZ() * cMo[1][2] + cMo[1][3]; diff --git a/modules/vision/src/pose-estimation/vpPoseLagrange.cpp b/modules/vision/src/pose-estimation/vpPoseLagrange.cpp index 56f5bbd847..12782da549 100644 --- a/modules/vision/src/pose-estimation/vpPoseLagrange.cpp +++ b/modules/vision/src/pose-estimation/vpPoseLagrange.cpp @@ -51,8 +51,8 @@ static void calculTranslation(vpMatrix &a, vpMatrix &b, unsigned int nl, unsigne unsigned int i, j; vpMatrix ct(3, nl); - for (i = 0; i < 3; i++) { - for (j = 0; j < nl; j++) + for (i = 0; i < 3; ++i) { + for (j = 0; j < nl; ++j) ct[i][j] = b[j][i + nc3]; } @@ -80,12 +80,12 @@ static void calculTranslation(vpMatrix &a, vpMatrix &b, unsigned int nl, unsigne vpColVector X2(nc3); vpMatrix CTB(nc1, nc3); - for (i = 0; i < nc1; i++) { - for (j = 0; j < nc3; j++) + for (i = 0; i < nc1; ++i) { + for (j = 0; j < nc3; ++j) CTB[i][j] = ctb[i][j]; } - for (j = 0; j < nc3; j++) + for (j = 0; j < nc3; ++j) X2[j] = x2[j]; vpColVector sv; // C^T A X1 + C^T B X2) @@ -102,7 +102,7 @@ static void calculTranslation(vpMatrix &a, vpMatrix &b, unsigned int nl, unsigne std::cout << "x3 " << X3.t(); #endif - for (i = 0; i < nc1; i++) + for (i = 0; i < nc1; ++i) x2[i + nc3] = X3[i]; } catch (...) { @@ -204,13 +204,13 @@ static void lagrange(vpMatrix &a, vpMatrix &b, vpColVector &x1, vpColVector &x2) imin = 0; // FC : Pourquoi calculer SVmax ?????? // double svm = 0.0; - // for (i=0;i svm) { svm = x1[i]; imin = i; } // } // svm *= EPS; /* pour le rang */ - for (i = 0; i < x1.getRows(); i++) + for (i = 0; i < x1.getRows(); ++i) if (x1[i] < x1[imin]) imin = i; @@ -220,7 +220,7 @@ static void lagrange(vpMatrix &a, vpMatrix &b, vpColVector &x1, vpColVector &x2) std::cout << " i_min " << imin << std::endl; } #endif - for (i = 0; i < x1.getRows(); i++) + for (i = 0; i < x1.getRows(); ++i) x1[i] = ata[i][imin]; x2 = -(r * x1); // X_2 = - (B^T B)^(-1) B^T A X_1 @@ -313,7 +313,7 @@ void vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, bool *p_isPlan, double * r2 = vpColVector::crossProd(r3, r1); vpHomogeneousMatrix fMo; - for (unsigned int i = 0; i < 3; i++) { + for (unsigned int i = 0; i < 3; ++i) { fMo[0][i] = r1[i]; fMo[1][i] = r2[i]; fMo[2][i] = r3[i]; @@ -386,21 +386,21 @@ void vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, bool *p_isPlan, double * #endif if (X2[5] < 0.0) { /* to obtain Zo > 0 */ - for (unsigned int i = 0; i < 3; i++) + for (unsigned int i = 0; i < 3; ++i) X1[i] = -X1[i]; - for (unsigned int i = 0; i < 6; i++) + for (unsigned int i = 0; i < 6; ++i) X2[i] = -X2[i]; } double s = 0.0; - for (unsigned int i = 0; i < 3; i++) { + for (unsigned int i = 0; i < 3; ++i) { s += (X1[i] * X2[i]); } - for (unsigned int i = 0; i < 3; i++) { + for (unsigned int i = 0; i < 3; ++i) { X2[i] -= (s * X1[i]); } /* X1^T X2 = 0 */ // s = 0.0; - // for (i=0;i<3;i++) {s += (X2[i]*X2[i]);} + // for (i=0;i<3; ++i) {s += (X2[i]*X2[i]);} s = X2[0] * X2[0] + X2[1] * X2[1] + X2[2] * X2[2]; // To avoid a Coverity copy/past error if (s < 1e-10) { @@ -418,7 +418,7 @@ void vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, bool *p_isPlan, double * } s = 1.0 / sqrt(s); - for (unsigned int i = 0; i < 3; i++) { + for (unsigned int i = 0; i < 3; ++i) { X2[i] *= s; } /* X2^T X2 = 1 */ @@ -436,7 +436,7 @@ void vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, bool *p_isPlan, double * cMf[1][2] = (X1[2] * X2[0]) - (X1[0] * X2[2]); cMf[2][2] = (X1[0] * X2[1]) - (X1[1] * X2[0]); /* calcul de la matrice de passage */ - for (unsigned int i = 0; i < 3; i++) { + for (unsigned int i = 0; i < 3; ++i) { cMf[i][0] = X1[i]; cMf[i][1] = X2[i]; cMf[i][3] = X2[i + 3]; @@ -542,15 +542,15 @@ void vpPose::poseLagrangeNonPlan(vpHomogeneousMatrix &cMo) X2 *= -1; } s = 0.0; - for (i = 0; i < 3; i++) { + for (i = 0; i < 3; ++i) { s += (X1[i] * X2[i]); } - for (i = 0; i < 3; i++) { + for (i = 0; i < 3; ++i) { X2[i] -= (s * X1[i]); } /* X1^T X2 = 0 */ // s = 0.0; - // for (i=0;i<3;i++) {s += (X2[i]*X2[i]);} + // for (i=0;i<3; ++i) {s += (X2[i]*X2[i]);} s = X2[0] * X2[0] + X2[1] * X2[1] + X2[2] * X2[2]; // To avoid a Coverity copy/past error if (s < 1e-10) { @@ -569,7 +569,7 @@ void vpPose::poseLagrangeNonPlan(vpHomogeneousMatrix &cMo) } s = 1.0 / sqrt(s); - for (i = 0; i < 3; i++) { + for (i = 0; i < 3; ++i) { X2[i] *= s; } /* X2^T X2 = 1 */ @@ -579,7 +579,7 @@ void vpPose::poseLagrangeNonPlan(vpHomogeneousMatrix &cMo) calculTranslation(a, b, nl, 3, 6, X1, X2); - for (i = 0; i < 3; i++) { + for (i = 0; i < 3; ++i) { cMo[i][0] = X1[i]; cMo[i][1] = X2[i]; cMo[i][2] = X2[i + 3]; diff --git a/modules/vision/src/pose-estimation/vpPoseLowe.cpp b/modules/vision/src/pose-estimation/vpPoseLowe.cpp index 00b66baabe..030035c320 100644 --- a/modules/vision/src/pose-estimation/vpPoseLowe.cpp +++ b/modules/vision/src/pose-estimation/vpPoseLowe.cpp @@ -96,7 +96,7 @@ void eval_function(int npt, double *xc, double *f) vpRotationMatrix rd(u[0], u[1], u[2]); // rot_mat(u,rd); /* matrice de rotation correspondante */ - for (i = 0; i < npt; i++) { + for (i = 0; i < npt; ++i) { double x = rd[0][0] * XO[i] + rd[0][1] * YO[i] + rd[0][2] * ZO[i] + xc[0]; double y = rd[1][0] * XO[i] + rd[1][1] * YO[i] + rd[1][2] * ZO[i] + xc[1]; double z = rd[2][0] * XO[i] + rd[2][1] * YO[i] + rd[2][2] * ZO[i] + xc[2]; @@ -166,7 +166,7 @@ void fcn(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int i double mco = 1.0 - co; double si = sin(tt); - for (int i = 0; i < npt; i++) { + for (int i = 0; i < npt; ++i) { double x = XO[i]; double y = YO[i]; /* coordonnees du point i */ double z = ZO[i]; @@ -270,13 +270,13 @@ void vpPose::poseLowe(vpHomogeneousMatrix &cMo) tol = std::numeric_limits::epsilon(); /* critere d'arret */ // c = cam ; - // for (i=0;i<3;i++) - // for (j=0;j<3;j++) rd[i][j] = cMo[i][j]; + // for (i=0;i<3; ++i) + // for (j=0;j<3; ++j) rd[i][j] = cMo[i][j]; // mat_rot(rd,u); vpRotationMatrix cRo; cMo.extract(cRo); vpThetaUVector u(cRo); - for (unsigned int i = 0; i < 3; i++) { + for (unsigned int i = 0; i < 3; ++i) { sol[i] = cMo[i][3]; sol[i + 3] = u[i]; } @@ -299,10 +299,10 @@ void vpPose::poseLowe(vpHomogeneousMatrix &cMo) // return FATAL_ERROR ; } - for (unsigned int i = 0; i < 3; i++) + for (unsigned int i = 0; i < 3; ++i) u[i] = sol[i + 3]; - for (unsigned int i = 0; i < 3; i++) { + for (unsigned int i = 0; i < 3; ++i) { cMo[i][3] = sol[i]; u[i] = sol[i + 3]; } @@ -310,7 +310,7 @@ void vpPose::poseLowe(vpHomogeneousMatrix &cMo) vpRotationMatrix rd(u); cMo.insert(rd); // rot_mat(u,rd); - // for (i=0;i<3;i++) for (j=0;j<3;j++) cMo[i][j] = rd[i][j]; + // for (i=0;i<3; ++i) for (j=0;j<3; ++j) cMo[i][j] = rd[i][j]; #if (DEBUG_LEVEL1) std::cout << "end CCalculPose::PoseLowe(...) " << std::endl; diff --git a/modules/vision/src/pose-estimation/vpPoseRGBD.cpp b/modules/vision/src/pose-estimation/vpPoseRGBD.cpp index 8de618bc7e..f6b8ffcbe8 100644 --- a/modules/vision/src/pose-estimation/vpPoseRGBD.cpp +++ b/modules/vision/src/pose-estimation/vpPoseRGBD.cpp @@ -55,7 +55,7 @@ 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++) { + for (unsigned int iter = 0; iter < max_iter && std::fabs(error - prev_error) > 1e-6; ++iter) { if (iter != 0) { tukey.MEstimator(vpRobust::TUKEY, residues, weights); } @@ -64,7 +64,7 @@ void estimatePlaneEquationSVD(const std::vector &point_cloud_face, vpPla double centroid_x = 0.0, centroid_y = 0.0, centroid_z = 0.0; double total_w = 0.0; - for (unsigned int i = 0; i < nPoints; i++) { + for (unsigned int i = 0; i < nPoints; ++i) { centroid_x += weights[i] * point_cloud_face[3 * i + 0]; centroid_y += weights[i] * point_cloud_face[3 * i + 1]; centroid_z += weights[i] * point_cloud_face[3 * i + 2]; @@ -76,7 +76,7 @@ void estimatePlaneEquationSVD(const std::vector &point_cloud_face, vpPla centroid_z /= total_w; // Minimization - for (unsigned int i = 0; i < nPoints; i++) { + for (unsigned int i = 0; i < nPoints; ++i) { M[static_cast(i)][0] = weights[i] * (point_cloud_face[3 * i + 0] - centroid_x); M[static_cast(i)][1] = weights[i] * (point_cloud_face[3 * i + 1] - centroid_y); M[static_cast(i)][2] = weights[i] * (point_cloud_face[3 * i + 2] - centroid_z); @@ -89,7 +89,7 @@ void estimatePlaneEquationSVD(const std::vector &point_cloud_face, vpPla double smallestSv = W[0]; unsigned int indexSmallestSv = 0; - for (unsigned int i = 1; i < W.size(); i++) { + for (unsigned int i = 1; i < W.size(); ++i) { if (W[i] < smallestSv) { smallestSv = W[i]; indexSmallestSv = i; @@ -105,7 +105,7 @@ void estimatePlaneEquationSVD(const std::vector &point_cloud_face, vpPla // Compute error points to estimated plane prev_error = error; error = 0.0; - for (unsigned int i = 0; i < nPoints; i++) { + for (unsigned int i = 0; i < nPoints; ++i) { residues[i] = std::fabs(A * point_cloud_face[3 * i] + B * point_cloud_face[3 * i + 1] + C * point_cloud_face[3 * i + 2] + D) / sqrt(A * A + B * B + C * C); @@ -121,7 +121,7 @@ void estimatePlaneEquationSVD(const std::vector &point_cloud_face, vpPla centroid.resize(3, false); double total_w = 0.0; - for (unsigned int i = 0; i < nPoints; i++) { + for (unsigned int i = 0; i < nPoints; ++i) { centroid[0] += weights[i] * point_cloud_face[3 * i]; centroid[1] += weights[i] * point_cloud_face[3 * i + 1]; centroid[2] += weights[i] * point_cloud_face[3 * i + 2]; @@ -159,7 +159,7 @@ bool vpPose::computePlanarObjectPoseFromRGBD(const vpImage &depthMap, con *confidence_index = 0.0; } - for (size_t i = 0; i < point3d.size(); i++) { + for (size_t i = 0; i < point3d.size(); ++i) { pose_points.push_back(point3d[i]); } @@ -174,8 +174,8 @@ bool vpPose::computePlanarObjectPoseFromRGBD(const vpImage &depthMap, con std::vector points_3d; points_3d.reserve((bottom - top) * (right - left)); - for (unsigned int idx_i = top; idx_i < bottom; idx_i++) { - for (unsigned int idx_j = left; idx_j < right; idx_j++) { + for (unsigned int idx_i = top; idx_i < bottom; ++idx_i) { + for (unsigned int idx_j = left; idx_j < right; ++idx_j) { vpImagePoint imPt(idx_i, idx_j); if (depthMap[idx_i][idx_j] > 0 && polygon.isInside(imPt)) { double x = 0, y = 0; @@ -199,7 +199,7 @@ bool vpPose::computePlanarObjectPoseFromRGBD(const vpImage &depthMap, con double normalized_weights = 0; estimatePlaneEquationSVD(points_3d, plane_equation, centroid, normalized_weights); - for (size_t j = 0; j < corners.size(); j++) { + for (size_t j = 0; j < corners.size(); ++j) { const vpImagePoint &imPt = corners[j]; double x = 0, y = 0; vpPixelMeterConversion::convertPoint(colorIntrinsics, imPt.get_u(), imPt.get_v(), x, y); @@ -213,7 +213,7 @@ bool vpPose::computePlanarObjectPoseFromRGBD(const vpImage &depthMap, con pose_points[j].set_y(y); } - for (size_t i = 0; i < point3d.size(); i++) { + for (size_t i = 0; i < point3d.size(); ++i) { q.push_back(point3d[i]); } @@ -251,9 +251,9 @@ bool vpPose::computePlanarObjectPoseFromRGBD(const vpImage &depthMap, *confidence_index = 0.0; } - for (size_t i = 0; i < point3d.size(); i++) { + for (size_t i = 0; i < point3d.size(); ++i) { std::vector tagPoint3d = point3d[i]; - for (size_t j = 0; j < tagPoint3d.size(); j++) { + for (size_t j = 0; j < tagPoint3d.size(); ++j) { pose_points.push_back(tagPoint3d[j]); } } @@ -269,7 +269,7 @@ bool vpPose::computePlanarObjectPoseFromRGBD(const vpImage &depthMap, size_t nb_points_3d_non_planar = 0; // Loop through each object, compute 3d point cloud of each - for (size_t i = 0; i < corners.size(); i++) { + for (size_t i = 0; i < corners.size(); ++i) { std::vector points_3d; vpPolygon polygon(corners[i]); vpRect bb = polygon.getBoundingBox(); @@ -285,8 +285,8 @@ bool vpPose::computePlanarObjectPoseFromRGBD(const vpImage &depthMap, static_cast(std::min(static_cast(depthMap.getWidth()) - 1, static_cast(bb.getRight()))); points_3d.reserve((bottom - top) * (right - left)); - for (unsigned int idx_i = top; idx_i < bottom; idx_i++) { - for (unsigned int idx_j = left; idx_j < right; idx_j++) { + for (unsigned int idx_i = top; idx_i < bottom; ++idx_i) { + for (unsigned int idx_j = left; idx_j < right; ++idx_j) { vpImagePoint imPt(idx_i, idx_j); if (depthMap[idx_i][idx_j] > 0 && polygon.isInside(imPt)) { double x = 0, y = 0; @@ -331,9 +331,9 @@ bool vpPose::computePlanarObjectPoseFromRGBD(const vpImage &depthMap, // If all objects are coplanar, use points insides tag_points_3d to estimate the plane estimatePlaneEquationSVD(tag_points_3d, plane_equation, centroid, normalized_weights); int count = 0; - for (size_t j = 0; j < corners.size(); j++) { + for (size_t j = 0; j < corners.size(); ++j) { std::vector tag_corner = corners[j]; - for (size_t i = 0; i < tag_corner.size(); i++) { + for (size_t i = 0; i < tag_corner.size(); ++i) { const vpImagePoint &imPt = tag_corner[i]; double x = 0, y = 0; vpPixelMeterConversion::convertPoint(colorIntrinsics, imPt.get_u(), imPt.get_v(), x, y); @@ -353,7 +353,7 @@ bool vpPose::computePlanarObjectPoseFromRGBD(const vpImage &depthMap, // If the tags is not coplanar, estimate the plane for each tags size_t count = 0; - for (size_t k = 0; k < tag_points_3d_nonplanar.size(); k++) { + for (size_t k = 0; k < tag_points_3d_nonplanar.size(); ++k) { std::vector rec_points_3d = tag_points_3d_nonplanar[k]; double tag_normalized_weights = 0; @@ -365,7 +365,7 @@ bool vpPose::computePlanarObjectPoseFromRGBD(const vpImage &depthMap, // Get the 2d points of the tag the plane just recomputed std::vector tag_corner = corners[k]; - for (size_t i = 0; i < tag_corner.size(); i++) { + for (size_t i = 0; i < tag_corner.size(); ++i) { const vpImagePoint &imPt = tag_corner[i]; double x = 0, y = 0; vpPixelMeterConversion::convertPoint(colorIntrinsics, imPt.get_u(), imPt.get_v(), x, y); @@ -390,20 +390,20 @@ bool vpPose::computePlanarObjectPoseFromRGBD(const vpImage &depthMap, normalized_weights = normalized_weights / tag_points_3d_nonplanar.size(); } - for (size_t i = 0; i < point3d.size(); i++) { + for (size_t i = 0; i < point3d.size(); ++i) { std::vector tagPoint3d = point3d[i]; // Sometimes an object may do not have enough points registered due to small size. // The issue happens with Orbbec camera while Realsenses was fine. // To prevent wrong estimation or exception (p and q sizes are differents), // ignore the recomputer vector (tag_points_3d_nonplanar) when size = 0 if (coplanar_points) { - for (size_t j = 0; j < tagPoint3d.size(); j++) { + for (size_t j = 0; j < tagPoint3d.size(); ++j) { q.push_back(tagPoint3d[j]); } } else { if (tag_points_3d_nonplanar[i].size() > 0) { - for (size_t j = 0; j < tagPoint3d.size(); j++) { + for (size_t j = 0; j < tagPoint3d.size(); ++j) { q.push_back(tagPoint3d[j]); } } diff --git a/modules/vision/src/pose-estimation/vpPoseRansac.cpp b/modules/vision/src/pose-estimation/vpPoseRansac.cpp index 371790988c..c4702296e9 100644 --- a/modules/vision/src/pose-estimation/vpPoseRansac.cpp +++ b/modules/vision/src/pose-estimation/vpPoseRansac.cpp @@ -255,7 +255,7 @@ bool vpPose::vpRansacFunctor::poseRansacImpl() unsigned int nbInliersCur = 0; unsigned int iter = 0; for (std::vector::const_iterator it = m_listOfUniquePoints.begin(); it != m_listOfUniquePoints.end(); - ++it, iter++) { + ++it, ++iter) { p.setWorldCoordinates(it->get_oX(), it->get_oY(), it->get_oZ()); p.track(m_cMo); @@ -341,7 +341,7 @@ bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneo std::map filterObjectPointMap; size_t index_pt = 0; for (std::vector::const_iterator it_pt = listOfPoints.begin(); it_pt != listOfPoints.end(); - ++it_pt, index_pt++) { + ++it_pt, ++index_pt) { if (filterObjectPointMap.find(*it_pt) == filterObjectPointMap.end()) { filterObjectPointMap[*it_pt] = index_pt; } @@ -364,7 +364,7 @@ bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneo size_t index_pt = 0; for (std::vector::const_iterator it_pt = listOfPoints.begin(); it_pt != listOfPoints.end(); - ++it_pt, index_pt++) { + ++it_pt, ++index_pt) { mapOfUniquePointIndex[index_pt] = index_pt; } } @@ -404,9 +404,9 @@ bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneo std::vector ransacWorkers; int splitTrials = ransacMaxTrials / nbThreads; - for (size_t i = 0; i < (size_t)nbThreads; i++) { - unsigned int initial_seed = (unsigned int)i; //((unsigned int) time(nullptr) ^ i); - if (i < (size_t)nbThreads - 1) { + for (size_t i = 0; i < static_cast(nbThreads); ++i) { + unsigned int initial_seed = static_cast(i); //((unsigned int) time(nullptr) ^ i); + if (i < static_cast(nbThreads) - 1) { ransacWorkers.emplace_back(cMo, ransacNbInlierConsensus, splitTrials, ransacThreshold, initial_seed, checkDegeneratePoints, listOfUniquePoints, func); } @@ -463,7 +463,7 @@ bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneo // Display the random picked points /* std::cout << "Randoms : " - for(unsigned int i = 0 ; i < cur_randoms.size() ; i++) + for(unsigned int i = 0 ; i < cur_randoms.size() ; ++i) std::cout << cur_randoms[i] << " " std::cout << std::endl */ @@ -471,7 +471,7 @@ bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneo // Display the outliers /* std::cout << "Outliers : " - for(unsigned int i = 0 ; i < cur_outliers.size() ; i++) + for(unsigned int i = 0 ; i < cur_outliers.size() ; ++i) std::cout << cur_outliers[i] << " " std::cout << std::endl */ @@ -487,7 +487,7 @@ bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneo // with VVS pose estimation vpPose pose; size_t best_consensus_size = best_consensus.size(); - for (size_t i = 0; i < best_consensus_size; i++) { + for (size_t i = 0; i < best_consensus_size; ++i) { vpPoint pt = listOfUniquePoints[best_consensus[i]]; pose.addPoint(pt); @@ -570,8 +570,8 @@ void vpPose::findMatch(std::vector &p2D, std::vector &p3D, unsigned int p2D_size = p2D.size(); unsigned int p3D_size = p3D.size(); - for (unsigned int i = 0; i < p2D_size; i++) { - for (unsigned int j = 0; j < p3D_size; j++) { + for (unsigned int i = 0; i < p2D_size; ++i) { + for (unsigned int j = 0; j < p3D_size; ++j) { vpPoint pt(p3D[j].getWorldCoordinates()); pt.set_x(p2D[i].get_x()); pt.set_y(p2D[i].get_y()); diff --git a/modules/vision/src/pose-estimation/vpPoseVirtualVisualServoing.cpp b/modules/vision/src/pose-estimation/vpPoseVirtualVisualServoing.cpp index 9a8b6c1c48..14ba955312 100644 --- a/modules/vision/src/pose-estimation/vpPoseVirtualVisualServoing.cpp +++ b/modules/vision/src/pose-estimation/vpPoseVirtualVisualServoing.cpp @@ -214,13 +214,13 @@ void vpPose::poseVirtualVSrobust(vpHomogeneousMatrix &cMo) // compute the residual r = error.sumSquare(); - for (unsigned int k = 0; k < error.getRows() / 2; k++) { + for (unsigned int k = 0; k < error.getRows() / 2; ++k) { res[k] = vpMath::sqr(error[2 * k]) + vpMath::sqr(error[2 * k + 1]); } robust.MEstimator(vpRobust::TUKEY, res, w); // compute the pseudo inverse of the interaction matrix - for (unsigned int k = 0; k < error.getRows() / 2; k++) { + for (unsigned int k = 0; k < error.getRows() / 2; ++k) { W[2 * k][2 * k] = w[k]; W[2 * k + 1][2 * k + 1] = w[k]; } @@ -262,7 +262,7 @@ std::optional vpPose::poseVirtualVSWithDepth(const std::vec vpColVector sd(3 * nb), s(3 * nb); // create sd - for (auto i = 0u; i < points.size(); i++) { + for (auto i = 0u; i < points.size(); ++i) { sd[3 * i] = points[i].get_x(); sd[3 * i + 1] = points[i].get_y(); sd[3 * i + 2] = points[i].get_Z(); @@ -274,7 +274,7 @@ std::optional vpPose::poseVirtualVSWithDepth(const std::vec residu_1 = r; // Compute the interaction matrix and the error - for (auto i = 0u; i < points.size(); i++) { + for (auto i = 0u; i < points.size(); ++i) { // forward projection of the 3D model for a given pose // change frame coordinates // perspective projection From 5c245b0258c506756240d13c2e22cac216d3a289 Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Wed, 10 Apr 2024 16:55:42 +0200 Subject: [PATCH 16/17] Fix issues introduced in previous commit --- .../io/src/image/private/vpImageIoLibjpeg.cpp | 16 ++++---- .../io/src/image/private/vpImageIoLibpng.cpp | 40 +++++++++---------- modules/tracker/blob/src/dots/vpDot2.cpp | 21 +++++----- 3 files changed, 39 insertions(+), 38 deletions(-) diff --git a/modules/io/src/image/private/vpImageIoLibjpeg.cpp b/modules/io/src/image/private/vpImageIoLibjpeg.cpp index 223b7fc4ea..692183d742 100644 --- a/modules/io/src/image/private/vpImageIoLibjpeg.cpp +++ b/modules/io/src/image/private/vpImageIoLibjpeg.cpp @@ -228,10 +228,10 @@ void readJPEGLibjpeg(vpImage &I, const std::string &filename) while (cinfo.output_scanline < cinfo.output_height) { jpeg_read_scanlines(&cinfo, buffer, 1); for (unsigned int i = 0; i < width; ++i) { - *(++output) = buffer[0][i * 3]; - *(++output) = buffer[0][i * 3 + 1]; - *(++output) = buffer[0][i * 3 + 2]; - *(++output) = vpRGBa::alpha_default; + *(output++) = buffer[0][i * 3]; + *(output++) = buffer[0][i * 3 + 1]; + *(output++) = buffer[0][i * 3 + 2]; + *(output++) = vpRGBa::alpha_default; } } vpImageConvert::convert(Ic, I); @@ -308,10 +308,10 @@ void readJPEGLibjpeg(vpImage &I, const std::string &filename) while (cinfo.output_scanline < cinfo.output_height) { jpeg_read_scanlines(&cinfo, buffer, 1); for (unsigned int i = 0; i < width; ++i) { - *(++output) = buffer[0][i * 3]; - *(++output) = buffer[0][i * 3 + 1]; - *(++output) = buffer[0][i * 3 + 2]; - *(++output) = vpRGBa::alpha_default; + *(output++) = buffer[0][i * 3]; + *(output++) = buffer[0][i * 3 + 1]; + *(output++) = buffer[0][i * 3 + 2]; + *(output++) = vpRGBa::alpha_default; } } } diff --git a/modules/io/src/image/private/vpImageIoLibpng.cpp b/modules/io/src/image/private/vpImageIoLibpng.cpp index 884c4752f6..e7b3afcc5b 100644 --- a/modules/io/src/image/private/vpImageIoLibpng.cpp +++ b/modules/io/src/image/private/vpImageIoLibpng.cpp @@ -381,24 +381,24 @@ void readPNGLibpng(vpImage &I, const std::string &filename) case 1: output = (unsigned char *)I.bitmap; for (unsigned int i = 0; i < width * height; ++i) { - *(++output) = data[i]; + *(output++) = data[i]; } break; case 2: output = (unsigned char *)I.bitmap; for (unsigned int i = 0; i < width * height; ++i) { - *(++output) = data[i * 2]; + *(output++) = data[i * 2]; } break; case 3: output = (unsigned char *)Ic.bitmap; for (unsigned int i = 0; i < width * height; ++i) { - *(++output) = data[i * 3]; - *(++output) = data[i * 3 + 1]; - *(++output) = data[i * 3 + 2]; - *(++output) = vpRGBa::alpha_default; + *(output++) = data[i * 3]; + *(output++) = data[i * 3 + 1]; + *(output++) = data[i * 3 + 2]; + *(output++) = vpRGBa::alpha_default; } vpImageConvert::convert(Ic, I); break; @@ -406,10 +406,10 @@ void readPNGLibpng(vpImage &I, const std::string &filename) case 4: output = (unsigned char *)Ic.bitmap; for (unsigned int i = 0; i < width * height; ++i) { - *(++output) = data[i * 4]; - *(++output) = data[i * 4 + 1]; - *(++output) = data[i * 4 + 2]; - *(++output) = data[i * 4 + 3]; + *(output++) = data[i * 4]; + *(output++) = data[i * 4 + 1]; + *(output++) = data[i * 4 + 2]; + *(output++) = data[i * 4 + 3]; } vpImageConvert::convert(Ic, I); break; @@ -558,7 +558,7 @@ void readPNGLibpng(vpImage &I, const std::string &filename) case 1: output = (unsigned char *)Ig.bitmap; for (unsigned int i = 0; i < width * height; ++i) { - *(++output) = data[i]; + *(output++) = data[i]; } vpImageConvert::convert(Ig, I); break; @@ -566,7 +566,7 @@ void readPNGLibpng(vpImage &I, const std::string &filename) case 2: output = (unsigned char *)Ig.bitmap; for (unsigned int i = 0; i < width * height; ++i) { - *(++output) = data[i * 2]; + *(output++) = data[i * 2]; } vpImageConvert::convert(Ig, I); break; @@ -574,20 +574,20 @@ void readPNGLibpng(vpImage &I, const std::string &filename) case 3: output = (unsigned char *)I.bitmap; for (unsigned int i = 0; i < width * height; ++i) { - *(++output) = data[i * 3]; - *(++output) = data[i * 3 + 1]; - *(++output) = data[i * 3 + 2]; - *(++output) = vpRGBa::alpha_default; + *(output++) = data[i * 3]; + *(output++) = data[i * 3 + 1]; + *(output++) = data[i * 3 + 2]; + *(output++) = vpRGBa::alpha_default; } break; case 4: output = (unsigned char *)I.bitmap; for (unsigned int i = 0; i < width * height; ++i) { - *(++output) = data[i * 4]; - *(++output) = data[i * 4 + 1]; - *(++output) = data[i * 4 + 2]; - *(++output) = data[i * 4 + 3]; + *(output++) = data[i * 4]; + *(output++) = data[i * 4 + 1]; + *(output++) = data[i * 4 + 2]; + *(output++) = data[i * 4 + 3]; } break; } diff --git a/modules/tracker/blob/src/dots/vpDot2.cpp b/modules/tracker/blob/src/dots/vpDot2.cpp index ccf57ba60f..5f980c1178 100644 --- a/modules/tracker/blob/src/dots/vpDot2.cpp +++ b/modules/tracker/blob/src/dots/vpDot2.cpp @@ -258,7 +258,7 @@ void vpDot2::initTracking(const vpImage &I, unsigned int size) unsigned int i = static_cast(cog.get_i()); unsigned int j = static_cast(cog.get_j()); - double Ip = pow(static_cast(I[i][j] / 255), 1 / gamma); + double Ip = pow(static_cast(I[i][j]) / 255, 1 / gamma); if ((Ip - (1 - grayLevelPrecision)) < 0) { gray_level_min = 0; @@ -572,6 +572,7 @@ void vpDot2::track(const vpImage &I, bool canMakeTheWindowGrow) gray_level_max = 255; } + // printf("%i %i \n",gray_level_max,gray_level_min); if (graphics) { // display a red cross at the center of gravity's location in the image. vpDisplay::displayCross(I, this->cog, (3 * thickness) + 8, vpColor::red, thickness); @@ -1343,12 +1344,12 @@ bool vpDot2::isValid(const vpImage &I, const vpDot2 &wantedDot) // a2^2 = 2 / m00 * (mu02 + mu20 - sqrt( (mu20 - mu02)^2 + 4mu11^2) ) // we compute parameters of the estimated ellipse - double tmp1 = (((m01 * m01) - (m10 * m10)) / m00) + (m20 - m02); - double tmp2 = (m11 - m10) * (m01 / m00); - double Sqrt = sqrt((tmp1 * tmp1) + (4 * tmp2 * tmp2)); - double a1 = sqrt((2 / m00) * ((m20 + m02) - (((m10 * m10) + (m01 * m01)) / m00) + Sqrt)); - double a2 = sqrt((2 / m00) * ((m20 + m02) - (((m10 * m10) + (m01 * m01)) / m00) - Sqrt)); - double alpha = 0.5 * atan2(2 * ((m11 * m00) - (m10 * m01)), (((m20 - m02) * m00) - (m10 * m10) + (m01 * m01))); + double tmp1 = (m01 * m01 - m10 * m10) / m00 + (m20 - m02); + double tmp2 = m11 - m10 * m01 / m00; + double Sqrt = sqrt(tmp1 * tmp1 + 4 * tmp2 * tmp2); + double a1 = sqrt(2 / m00 * ((m20 + m02) - (m10 * m10 + m01 * m01) / m00 + Sqrt)); + double a2 = sqrt(2 / m00 * ((m20 + m02) - (m10 * m10 + m01 * m01) / m00 - Sqrt)); + double alpha = 0.5 * atan2(2 * (m11 * m00 - m10 * m01), ((m20 - m02) * m00 - m10 * m10 + m01 * m01)); // to be able to track small dots, minorize the ellipsoid radius for the // inner test @@ -1371,6 +1372,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 + // return false; nb_bad_points++; } if (graphics) { @@ -1486,11 +1488,10 @@ bool vpDot2::hasGoodLevel(const vpImage &I, const unsigned int &u bool vpDot2::hasReverseLevel(const vpImage &I, const unsigned int &u, const unsigned int &v) const { - if (!isInArea(u, v)) { + if (!isInArea(u, v)) return false; - } - if ((I[v][u] < gray_level_min) || (I[v][u] > gray_level_max)) { + if (I[v][u] < gray_level_min || I[v][u] > gray_level_max) { return true; } else { From 2de4a65db8980f91bb605b164dce8683ddad2e68 Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Thu, 11 Apr 2024 08:48:55 +0200 Subject: [PATCH 17/17] Enable ctest verbose mode on appveyor ci --- appveyor.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/appveyor.yml b/appveyor.yml index 13430ea240..715925da71 100644 --- a/appveyor.yml +++ b/appveyor.yml @@ -119,7 +119,7 @@ build_script: - if "%TARGET%"=="mingw" cmake --build . --config %configuration% --target install -- -j2 - if "%TARGET%"=="mingw" dir C:\projects\visp\build\install - if "%TARGET%"=="mingw" dir %VISP_DLL_DIR% - - if "%TARGET%"=="mingw" ctest --output-on-failure + - if "%TARGET%"=="mingw" ctest --output-on-failure -V # msvc case - if "%TARGET%"=="msvc" dir %OpenCV_DLL_DIR% @@ -127,7 +127,7 @@ build_script: - if "%TARGET%"=="msvc" cmake --build . --config %configuration% --target install -- /m:2 - if "%TARGET%"=="msvc" dir C:\projects\visp\build\install - if "%TARGET%"=="msvc" dir %VISP_DLL_DIR% - - if "%TARGET%"=="msvc" ctest --output-on-failure + - if "%TARGET%"=="msvc" ctest --output-on-failure -V # uwp case - if "%TARGET%"=="uwp" cmake -G "Visual Studio 15 2017" -A %platform% -DCMAKE_SYSTEM_NAME="WindowsStore" -DCMAKE_SYSTEM_VERSION="10.0" -DBUILD_DEMOS=OFF -DBUILD_EXAMPLES=OFF -DBUILD_TESTS=OFF -DBUILD_TUTORIALS=OFF ..\visp