From 3f3e3f2c76c5888762274a43a9ddd56922fa7cc0 Mon Sep 17 00:00:00 2001 From: Transporter Date: Fri, 5 Jan 2024 10:44:50 +0100 Subject: [PATCH 1/4] Fix std::min/std::max template types --- modules/core/include/visp3/core/vpMath.h | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/modules/core/include/visp3/core/vpMath.h b/modules/core/include/visp3/core/vpMath.h index 19fdade01f..75727fbdd1 100644 --- a/modules/core/include/visp3/core/vpMath.h +++ b/modules/core/include/visp3/core/vpMath.h @@ -488,14 +488,14 @@ template <> inline unsigned char vpMath::saturate(char v) // On little endian arch, CHAR_MIN=-127 and CHAR_MAX=128 leading to // (int)(char -127) = -127. if (std::numeric_limits::is_signed) - return static_cast(std::max(static_cast(v), 0)); + return static_cast(std::max(static_cast(v), 0)); else return static_cast(static_cast(v) > SCHAR_MAX ? 0 : v); } template <> inline unsigned char vpMath::saturate(unsigned short v) { - return static_cast(std::min(static_cast(v), static_cast(UCHAR_MAX))); + return static_cast(std::min(static_cast(v), static_cast(UCHAR_MAX))); } template <> inline unsigned char vpMath::saturate(int v) @@ -510,7 +510,7 @@ template <> inline unsigned char vpMath::saturate(short v) template <> inline unsigned char vpMath::saturate(unsigned int v) { - return static_cast(std::min(v, static_cast(UCHAR_MAX))); + return static_cast(std::min(v, static_cast(UCHAR_MAX))); } template <> inline unsigned char vpMath::saturate(float v) @@ -528,12 +528,12 @@ template <> inline unsigned char vpMath::saturate(double v) // char template <> inline char vpMath::saturate(unsigned char v) { - return static_cast(std::min(static_cast(v), SCHAR_MAX)); + return static_cast(std::min(static_cast(v), SCHAR_MAX)); } template <> inline char vpMath::saturate(unsigned short v) { - return static_cast(std::min(static_cast(v), static_cast(SCHAR_MAX))); + return static_cast(std::min(static_cast(v), static_cast(SCHAR_MAX))); } template <> inline char vpMath::saturate(int v) @@ -548,7 +548,7 @@ template <> inline char vpMath::saturate(short v) template <> inline char vpMath::saturate(unsigned int v) { - return static_cast(std::min(v, static_cast(SCHAR_MAX))); + return static_cast(std::min(v, static_cast(SCHAR_MAX))); } template <> inline char vpMath::saturate(float v) @@ -572,14 +572,14 @@ template <> inline unsigned short vpMath::saturate(char v) // On little endian arch, CHAR_MIN=-127 and CHAR_MAX=128 leading to // (int)(char -127) = -127. if (std::numeric_limits::is_signed) - return static_cast(std::max(static_cast(v), 0)); + return static_cast(std::max(static_cast(v), 0)); else return static_cast(static_cast(v) > SCHAR_MAX ? 0 : v); } template <> inline unsigned short vpMath::saturate(short v) { - return static_cast(std::max(static_cast(v), 0)); + return static_cast(std::max(static_cast(v), 0)); } template <> inline unsigned short vpMath::saturate(int v) @@ -589,7 +589,7 @@ template <> inline unsigned short vpMath::saturate(int v) template <> inline unsigned short vpMath::saturate(unsigned int v) { - return static_cast(std::min(v, static_cast(USHRT_MAX))); + return static_cast(std::min(v, static_cast(USHRT_MAX))); } template <> inline unsigned short vpMath::saturate(float v) @@ -607,7 +607,7 @@ template <> inline unsigned short vpMath::saturate(double v) // short template <> inline short vpMath::saturate(unsigned short v) { - return static_cast(std::min(static_cast(v), SHRT_MAX)); + return static_cast(std::min(static_cast(v), SHRT_MAX)); } template <> inline short vpMath::saturate(int v) { @@ -615,7 +615,7 @@ template <> inline short vpMath::saturate(int v) } template <> inline short vpMath::saturate(unsigned int v) { - return static_cast(std::min(v, static_cast(SHRT_MAX))); + return static_cast(std::min(v, static_cast(SHRT_MAX))); } template <> inline short vpMath::saturate(float v) { From 50cb067e4edde90bd2e963e9c4517874be371428 Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Fri, 5 Jan 2024 18:42:46 +0100 Subject: [PATCH 2/4] More fixes for std::min/std::max template types to prevent potential build issue with Microsoft Visual Studio 2022 --- modules/core/include/visp3/core/vpImage.h | 20 +- .../core/include/visp3/core/vpImageFilter.h | 4 +- .../core/include/visp3/core/vpImageTools.h | 28 +- modules/core/include/visp3/core/vpMunkres.h | 4 +- .../visp3/core/vpPixelMeterConversion.h | 4 +- modules/core/include/visp3/core/vpRect.h | 8 +- modules/core/src/display/vpDisplay.cpp | 6 +- modules/core/src/image/private/Font.hpp | 54 ++-- .../core/src/image/vpCannyEdgeDetection.cpp | 10 +- modules/core/src/image/vpFont.cpp | 12 +- modules/core/src/image/vpImageCircle.cpp | 16 +- modules/core/src/image/vpImageConvert.cpp | 8 +- modules/core/src/image/vpImageFilter.cpp | 2 +- modules/core/src/image/vpImageTools.cpp | 2 +- modules/core/src/math/matrix/vpColVector.cpp | 2 +- modules/core/src/math/matrix/vpMatrix.cpp | 8 +- modules/core/src/math/matrix/vpMatrix_qr.cpp | 65 +++-- modules/core/src/math/matrix/vpMatrix_svd.cpp | 10 +- modules/core/src/math/matrix/vpRowVector.cpp | 2 +- .../transformation/vpForceTwistMatrix.cpp | 8 +- .../src/math/transformation/vpPoseVector.cpp | 2 +- .../transformation/vpVelocityTwistMatrix.cpp | 8 +- modules/core/src/tools/file/vpIoTools.cpp | 2 +- .../core/src/tools/optimization/vpLinProg.cpp | 4 +- .../core/test/image-with-dataset/common.hpp | 72 +++-- modules/core/test/image/common.hpp | 74 ++--- modules/core/test/image/testImageGetValue.cpp | 50 ++-- .../test/tools/geometry/testImageCircle.cpp | 18 +- modules/gui/src/display/vpDisplayGTK.cpp | 8 +- modules/gui/src/display/vpDisplayOpenCV.cpp | 48 ++-- modules/gui/src/display/vpDisplayX.cpp | 44 +-- .../gui/src/display/windows/vpD3DRenderer.cpp | 64 +++-- .../gui/src/display/windows/vpGDIRenderer.cpp | 53 ++-- .../visp3/imgproc/vpCircleHoughTransform.h | 8 +- modules/imgproc/src/vpCLAHE.cpp | 28 +- .../imgproc/src/vpCircleHoughTransform.cpp | 57 ++-- modules/imgproc/src/vpMorph.cpp | 2 +- modules/imgproc/src/vpRetinex.cpp | 2 +- modules/imgproc/src/vpThreshold.cpp | 2 +- .../src/haptic-device/qbdevice/vpQbDevice.cpp | 2 +- .../src/real-robot/bebop2/vpRobotBebop2.cpp | 157 +++++++---- .../franka/vpJointVelTrajGenerator_impl.cpp | 266 +++++++++--------- .../test/servo-flir-ptu/testRobotFlirPtu.cpp | 118 ++++---- .../flycapture/vpFlyCaptureGrabber.cpp | 12 +- .../vpOccipitalStructure.cpp | 2 +- .../rgb-depth/testRealSense2_D435_align.cpp | 12 +- .../rgb-depth/testRealSense2_D435_opencv.cpp | 4 +- .../test/rgb-depth/testRealSense_R200.cpp | 4 +- .../test/rgb-depth/testRealSense_SR300.cpp | 2 +- .../mbt/src/depth/vpMbtFaceDepthDense.cpp | 24 +- .../mbt/src/depth/vpMbtFaceDepthNormal.cpp | 24 +- .../tracker/mbt/src/edge/vpMbEdgeTracker.cpp | 1 - .../tracker/mbt/src/edge/vpMbtMeEllipse.cpp | 2 +- modules/tracker/mbt/src/edge/vpMbtMeLine.cpp | 51 ++-- modules/tracker/mbt/src/vpMbScanLine.cpp | 45 +-- .../tracker/me/src/moving-edges/vpMeLine.cpp | 6 +- modules/vision/src/key-point/vpKeyPoint.cpp | 4 +- .../plane-estimation/vpPlaneEstimation.cpp | 12 +- .../private/vpLevenbergMarquartd.cpp | 2 +- .../src/pose-estimation/vpPoseDementhon.cpp | 2 +- .../vision/src/pose-estimation/vpPoseRGBD.cpp | 20 +- .../src/pose-estimation/vpPoseRansac.cpp | 10 +- tutorial/image/tutorial-canny.cpp | 2 +- .../flood-fill/tutorial-flood-fill.cpp | 27 +- ...l-megapose-live-single-object-tracking.cpp | 2 +- 65 files changed, 878 insertions(+), 754 deletions(-) diff --git a/modules/core/include/visp3/core/vpImage.h b/modules/core/include/visp3/core/vpImage.h index 0d5cbf9894..1259e7800c 100644 --- a/modules/core/include/visp3/core/vpImage.h +++ b/modules/core/include/visp3/core/vpImage.h @@ -1629,8 +1629,8 @@ template Type vpImage::getValue(double i, double j) const double rfrac = 1.0 - rratio; double cfrac = 1.0 - cratio; - unsigned int iround_1 = (std::min)(height - 1, iround + 1); - unsigned int jround_1 = (std::min)(width - 1, jround + 1); + unsigned int iround_1 = std::min(height - 1, iround + 1); + unsigned int jround_1 = std::min(width - 1, jround + 1); double value = (static_cast(row[iround][jround]) * rfrac + static_cast(row[iround_1][jround]) * rratio) * cfrac + @@ -1661,8 +1661,8 @@ template <> inline double vpImage::getValue(double i, double j) const double rfrac = 1.0 - rratio; double cfrac = 1.0 - cratio; - unsigned int iround_1 = (std::min)(height - 1, iround + 1); - unsigned int jround_1 = (std::min)(width - 1, jround + 1); + unsigned int iround_1 = std::min(height - 1, iround + 1); + unsigned int jround_1 = std::min(width - 1, jround + 1); return (row[iround][jround] * rfrac + row[iround_1][jround] * rratio) * cfrac + (row[iround][jround_1] * rfrac + row[iround_1][jround_1] * rratio) * cratio; @@ -1732,8 +1732,8 @@ template <> inline unsigned char vpImage::getValue(double i, doub double rfrac = 1.0 - rratio; double cfrac = 1.0 - cratio; - unsigned int iround_1 = (std::min)(height - 1, iround + 1); - unsigned int jround_1 = (std::min)(width - 1, jround + 1); + unsigned int iround_1 = std::min(height - 1, iround + 1); + unsigned int jround_1 = std::min(width - 1, jround + 1); double value = (static_cast(row[iround][jround]) * rfrac + static_cast(row[iround_1][jround]) * rratio) * cfrac + @@ -1764,8 +1764,8 @@ template <> inline vpRGBa vpImage::getValue(double i, double j) const double rfrac = 1.0 - rratio; double cfrac = 1.0 - cratio; - unsigned int iround_1 = (std::min)(height - 1, iround + 1); - unsigned int jround_1 = (std::min)(width - 1, jround + 1); + unsigned int iround_1 = std::min(height - 1, iround + 1); + unsigned int jround_1 = std::min(width - 1, jround + 1); double valueR = (static_cast(row[iround][jround].R) * rfrac + static_cast(row[iround_1][jround].R) * rratio) * @@ -1808,8 +1808,8 @@ template <> inline vpRGBf vpImage::getValue(double i, double j) const double rfrac = 1.0 - rratio; double cfrac = 1.0 - cratio; - unsigned int iround_1 = (std::min)(height - 1, iround + 1); - unsigned int jround_1 = (std::min)(width - 1, jround + 1); + unsigned int iround_1 = std::min(height - 1, iround + 1); + unsigned int jround_1 = std::min(width - 1, jround + 1); double valueR = (static_cast(row[iround][jround].R) * rfrac + static_cast(row[iround_1][jround].R) * rratio) * diff --git a/modules/core/include/visp3/core/vpImageFilter.h b/modules/core/include/visp3/core/vpImageFilter.h index fd6a28a4ca..aabada47f6 100644 --- a/modules/core/include/visp3/core/vpImageFilter.h +++ b/modules/core/include/visp3/core/vpImageFilter.h @@ -314,7 +314,7 @@ class VISP_EXPORT vpImageFilter float dx = static_cast(dIx[r][c]); float dy = static_cast(dIy[r][c]); float gradient = std::abs(dx) + std::abs(dy); - float gradientClamped = std::min(gradient, static_cast(std::numeric_limits::max())); + float gradientClamped = std::min(gradient, static_cast(std::numeric_limits::max())); dI[r][c] = static_cast(gradientClamped); } } @@ -334,7 +334,7 @@ class VISP_EXPORT vpImageFilter break; } } - float upperThresh = std::max(bon, 1.f); + float upperThresh = std::max(bon, 1.f); lowerThresh = lowerThresholdRatio * bon; return upperThresh; } diff --git a/modules/core/include/visp3/core/vpImageTools.h b/modules/core/include/visp3/core/vpImageTools.h index 864d9f6e4e..75a81034cf 100644 --- a/modules/core/include/visp3/core/vpImageTools.h +++ b/modules/core/include/visp3/core/vpImageTools.h @@ -304,10 +304,10 @@ template void vpImageTools::crop(const vpImage &I, double roi_top, double roi_left, unsigned int roi_height, unsigned int roi_width, vpImage &crop, unsigned int v_scale, unsigned int h_scale) { - int i_min = (std::max)((int)(ceil(roi_top / v_scale)), 0); - int j_min = (std::max)((int)(ceil(roi_left / h_scale)), 0); - int i_max = (std::min)((int)(ceil((roi_top + roi_height)) / v_scale), (int)(I.getHeight() / v_scale)); - int j_max = (std::min)((int)(ceil((roi_left + roi_width) / h_scale)), (int)(I.getWidth() / h_scale)); + int i_min = std::max((int)(ceil(roi_top / v_scale)), 0); + int j_min = std::max((int)(ceil(roi_left / h_scale)), 0); + int i_max = std::min((int)(ceil((roi_top + roi_height)) / v_scale), (int)(I.getHeight() / v_scale)); + int j_max = std::min((int)(ceil((roi_left + roi_width) / h_scale)), (int)(I.getWidth() / h_scale)); unsigned int i_min_u = (unsigned int)i_min; unsigned int j_min_u = (unsigned int)j_min; @@ -409,10 +409,10 @@ template void vpImageTools::crop(const unsigned char *bitmap, unsigned int width, unsigned int height, const vpRect &roi, vpImage &crop, unsigned int v_scale, unsigned int h_scale) { - int i_min = (std::max)((int)(ceil(roi.getTop() / v_scale)), 0); - int j_min = (std::max)((int)(ceil(roi.getLeft() / h_scale)), 0); - int i_max = (std::min)((int)(ceil((roi.getTop() + roi.getHeight()) / v_scale)), (int)(height / v_scale)); - int j_max = (std::min)((int)(ceil((roi.getLeft() + roi.getWidth()) / h_scale)), (int)(width / h_scale)); + int i_min = std::max((int)(ceil(roi.getTop() / v_scale)), 0); + int j_min = std::max((int)(ceil(roi.getLeft() / h_scale)), 0); + int i_max = std::min((int)(ceil((roi.getTop() + roi.getHeight()) / v_scale)), (int)(height / v_scale)); + int j_max = std::min((int)(ceil((roi.getLeft() + roi.getWidth()) / h_scale)), (int)(width / h_scale)); unsigned int i_min_u = (unsigned int)i_min; unsigned int j_min_u = (unsigned int)j_min; @@ -892,8 +892,8 @@ template Type vpImageTools::getPixelClamped(const vpImage &I, { int x = vpMath::round(u); int y = vpMath::round(v); - x = (std::max)(0, (std::min)(x, static_cast(I.getWidth()) - 1)); - y = (std::max)(0, (std::min)(y, static_cast(I.getHeight()) - 1)); + x = std::max(0, std::min(x, static_cast(I.getWidth()) - 1)); + y = std::max(0, std::min(y, static_cast(I.getHeight()) - 1)); return I[y][x]; } @@ -994,11 +994,11 @@ void vpImageTools::resizeBilinear(const vpImage &I, vpImage &Ires, u int u0 = static_cast(u); int v0 = static_cast(v); - int u1 = (std::min)(static_cast(I.getWidth()) - 1, u0 + 1); + int u1 = std::min(static_cast(I.getWidth()) - 1, u0 + 1); int v1 = v0; int u2 = u0; - int v2 = (std::min)(static_cast(I.getHeight()) - 1, v0 + 1); + int v2 = std::min(static_cast(I.getHeight()) - 1, v0 + 1); int u3 = u1; int v3 = v2; @@ -1017,11 +1017,11 @@ inline void vpImageTools::resizeBilinear(const vpImage &I, vpImage(u); int v0 = static_cast(v); - int u1 = (std::min)(static_cast(I.getWidth()) - 1, u0 + 1); + int u1 = std::min(static_cast(I.getWidth()) - 1, u0 + 1); int v1 = v0; int u2 = u0; - int v2 = (std::min)(static_cast(I.getHeight()) - 1, v0 + 1); + int v2 = std::min(static_cast(I.getHeight()) - 1, v0 + 1); int u3 = u1; int v3 = v2; diff --git a/modules/core/include/visp3/core/vpMunkres.h b/modules/core/include/visp3/core/vpMunkres.h index 78a35dd10c..802994da05 100644 --- a/modules/core/include/visp3/core/vpMunkres.h +++ b/modules/core/include/visp3/core/vpMunkres.h @@ -198,7 +198,7 @@ template inline vpMunkres::STEP_T vpMunkres::stepOne(std::vector for (auto col = 0u; col < costs.size(); ++col) { auto minval = std::numeric_limits::max(); for (const auto &cost_row : costs) { - minval = std::min(minval, cost_row.at(col)); + minval = std::min(minval, cost_row.at(col)); } for (auto &cost_row : costs) { @@ -316,7 +316,7 @@ inline std::vector > vpMunkres::run(std::v { const auto original_row_size = costs.size(); const auto original_col_size = costs.front().size(); - const auto sq_size = std::max(original_row_size, original_col_size); + const auto sq_size = std::max(original_row_size, original_col_size); auto mask = std::vector >( sq_size, std::vector(sq_size, vpMunkres::ZERO_T::NA)); diff --git a/modules/core/include/visp3/core/vpPixelMeterConversion.h b/modules/core/include/visp3/core/vpPixelMeterConversion.h index 962b3f8848..f1f0659747 100644 --- a/modules/core/include/visp3/core/vpPixelMeterConversion.h +++ b/modules/core/include/visp3/core/vpPixelMeterConversion.h @@ -272,7 +272,7 @@ class VISP_EXPORT vpPixelMeterConversion double scale = 1.0; double r_d = sqrt(vpMath::sqr(x_d) + vpMath::sqr(y_d)); - r_d = std::min(std::max(-M_PI, r_d), M_PI); // FOV restricted to 180degrees. + r_d = std::min(std::max(-M_PI, r_d), M_PI); // FOV restricted to 180degrees. std::vector k = cam.getKannalaBrandtDistortionCoefficients(); @@ -327,7 +327,7 @@ class VISP_EXPORT vpPixelMeterConversion double scale = 1.0; double r_d = sqrt(vpMath::sqr(x_d) + vpMath::sqr(y_d)); - r_d = std::min(std::max(-M_PI, r_d), M_PI); // FOV restricted to 180degrees. + r_d = std::min(std::max(-M_PI, r_d), M_PI); // FOV restricted to 180degrees. std::vector k = cam.getKannalaBrandtDistortionCoefficients(); diff --git a/modules/core/include/visp3/core/vpRect.h b/modules/core/include/visp3/core/vpRect.h index 985d0dbd73..5531af3a9d 100644 --- a/modules/core/include/visp3/core/vpRect.h +++ b/modules/core/include/visp3/core/vpRect.h @@ -239,10 +239,10 @@ class VISP_EXPORT vpRect */ inline vpRect &operator&=(const vpRect &r) { - double x1 = (std::max)(left, r.left); - double y1 = (std::max)(top, r.top); - width = (std::min)(left + width, r.left + r.width) - x1; - height = (std::min)(top + height, r.top + r.height) - y1; + double x1 = std::max(left, r.left); + double y1 = std::max(top, r.top); + width = std::min(left + width, r.left + r.width) - x1; + height = std::min(top + height, r.top + r.height) - y1; left = x1; top = y1; diff --git a/modules/core/src/display/vpDisplay.cpp b/modules/core/src/display/vpDisplay.cpp index 51b39c381d..ecabea2c95 100644 --- a/modules/core/src/display/vpDisplay.cpp +++ b/modules/core/src/display/vpDisplay.cpp @@ -243,9 +243,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 = (unsigned int)(std::max)(1u, (std::max)((unsigned int)wscale, (unsigned int)hscale)); + 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)); return scale; } diff --git a/modules/core/src/image/private/Font.hpp b/modules/core/src/image/private/Font.hpp index 77b87b37bc..d56b765951 100644 --- a/modules/core/src/image/private/Font.hpp +++ b/modules/core/src/image/private/Font.hpp @@ -65,7 +65,8 @@ #ifndef DOXYGEN_SHOULD_SKIP_THIS namespace Font { -template struct Point { +template struct Point +{ typedef T Type; /*!< Type definition. */ T x; /*!< \brief Specifies the x-coordinate of a point. */ @@ -110,19 +111,17 @@ template <> inline int Convert(double src) { return Round(src); } template <> inline int Convert(float src) { return Round(src); } -template inline Point::Point() : x(0), y(0) {} +template inline Point::Point() : x(0), y(0) { } template template inline Point::Point(TX tx, TY ty) : x(Convert(tx)), y(Convert(ty)) -{ -} +{ } template template class TPoint> inline Point::Point(const TPoint &p) : x(Convert(p.x)), y(Convert(p.y)) -{ -} +{ } template inline Point Point::operator<<(int shift) const { return Point(x << shift, y << shift); } @@ -140,7 +139,8 @@ template inline Point operator*(const TA &a, cons return Point(p.x * a, p.y * a); } -template struct Rectangle { +template struct Rectangle +{ typedef T Type; /*!< Type definition. */ T left; /*!< \brief Specifies the position of left side of a rectangle. */ @@ -362,36 +362,32 @@ template struct Rectangle { // struct Rectangle implementation: -template inline Rectangle::Rectangle() : left(0), top(0), right(0), bottom(0) {} +template inline Rectangle::Rectangle() : left(0), top(0), right(0), bottom(0) { } template template inline Rectangle::Rectangle(TL l, TT t, TR r, TB b) : left(Convert(l)), top(Convert(t)), right(Convert(r)), bottom(Convert(b)) -{ -} +{ } template template inline Rectangle::Rectangle(const Point <, const Point &rb) : left(Convert(lt.x)), top(Convert(lt.y)), right(Convert(rb.x)), bottom(Convert(rb.y)) -{ -} +{ } template template inline Rectangle::Rectangle(const Point &rb) : left(0), top(0), right(Convert(rb.x)), bottom(Convert(rb.y)) -{ -} +{ } template template class TRectangle> inline Rectangle::Rectangle(const TRectangle &r) : left(Convert(r.left)), top(Convert(r.top)), right(Convert(r.right)), - bottom(Convert(r.bottom)) -{ -} + bottom(Convert(r.bottom)) +{ } template template class TRectangle> @@ -473,10 +469,10 @@ template inline Rectangle Rectangle::Intersection(const Rectangle &rect) const { Rectangle _rect(rect); - T l = std::max(left, _rect.left); - T t = std::max(top, _rect.top); - T r = std::max(l, std::min(right, _rect.right)); - T b = std::max(t, std::min(bottom, _rect.bottom)); + T l = std::max(left, _rect.left); + T t = std::max(top, _rect.top); + T r = std::max(l, std::min(right, _rect.right)); + T b = std::max(t, std::min(bottom, _rect.bottom)); return Rectangle(l, t, r, b); } @@ -489,13 +485,13 @@ template template inline Rectangle &Rectangle:: Rectangle _r(r); if (left < _r.left) - left = std::min(_r.left, right); + left = std::min(_r.left, right); if (top < _r.top) - top = std::min(_r.top, bottom); + top = std::min(_r.top, bottom); if (right > _r.right) - right = std::max(_r.right, left); + right = std::max(_r.right, left); if (bottom > _r.bottom) - bottom = std::max(_r.bottom, top); + bottom = std::max(_r.bottom, top); return *this; } @@ -507,10 +503,10 @@ template template inline Rectangle &Rectangle:: return *this; Rectangle _r(r); - left = std::min(left, _r.left); - top = std::min(top, _r.top); - right = std::max(right, _r.right); - bottom = std::max(bottom, _r.bottom); + left = std::min(left, _r.left); + top = std::min(top, _r.top); + right = std::max(right, _r.right); + bottom = std::max(bottom, _r.bottom); return *this; } } // namespace Font diff --git a/modules/core/src/image/vpCannyEdgeDetection.cpp b/modules/core/src/image/vpCannyEdgeDetection.cpp index 884f2f02f5..9156591a15 100644 --- a/modules/core/src/image/vpCannyEdgeDetection.cpp +++ b/modules/core/src/image/vpCannyEdgeDetection.cpp @@ -198,7 +198,7 @@ vpCannyEdgeDetection::detect(const vpImage &I) lowerThreshold = m_upperThreshold / 3.f; } // To ensure that if lowerThreshold = 0, we reject null gradient points - lowerThreshold = std::max(lowerThreshold, std::numeric_limits::epsilon()); + lowerThreshold = std::max(lowerThreshold, std::numeric_limits::epsilon()); performEdgeThinning(lowerThreshold); // // Step 4: hysteresis thresholding @@ -338,8 +338,8 @@ getGradientOrientation(const vpImage &dIx, const vpImage &dIy, con else { // -dy because the y-axis of the image is oriented towards the bottom of the screen // while we later work with a y-axis oriented towards the top when getting the theta quadrant. - gradientOrientation = static_cast(std::atan2(-dy , dx)); - if(gradientOrientation < 0.f) { + gradientOrientation = static_cast(std::atan2(-dy, dx)); + if (gradientOrientation < 0.f) { gradientOrientation += M_PIf; // + M_PI in order to be between 0 and M_PIf } } @@ -427,9 +427,9 @@ vpCannyEdgeDetection::recursiveSearchForStrongEdge(const std::pair(idRow, 0); // Avoid getting negative pixel ID int idCol = dc + (int)coordinates.second; - idCol = std::max(idCol, 0); // Avoid getting negative pixel ID + 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) diff --git a/modules/core/src/image/vpFont.cpp b/modules/core/src/image/vpFont.cpp index ff72d5b283..fd706e8afb 100644 --- a/modules/core/src/image/vpFont.cpp +++ b/modules/core/src/image/vpFont.cpp @@ -197,8 +197,8 @@ class vpFont::Impl for (size_t i = 0; i < text.size(); i++) { if (text[i] >= _symbolMin && text[i] <= _symbolMax) { curr.x += _currentSize.x; - size.x = std::max(size.x, curr.x); - size.y = std::max(size.y, curr.y + _currentSize.y); + size.x = std::max(size.x, curr.x); + size.y = std::max(size.y, curr.y + _currentSize.y); } else if (text[i] == '\n') { curr.x = 0; @@ -317,8 +317,8 @@ class vpFont::Impl case TRUETYPE_FILE: { Measure(text); // Try to resize only if new size is bigger - _fontBuffer.resize(std::max(_fontBuffer.getHeight(), static_cast(_bb.getBottom() + 1)), - std::max(_fontBuffer.getWidth(), static_cast(_bb.getRight() + 1))); + _fontBuffer.resize(std::max(_fontBuffer.getHeight(), static_cast(_bb.getBottom() + 1)), + std::max(_fontBuffer.getWidth(), static_cast(_bb.getRight() + 1))); int wordUTF32_size = static_cast(_wordUTF32.size()); @@ -428,8 +428,8 @@ class vpFont::Impl case TRUETYPE_FILE: { Measure(text); // Try to resize only if new size is bigger - _fontBuffer.resize(std::max(_fontBuffer.getHeight(), static_cast(_bb.getBottom() + 1)), - std::max(_fontBuffer.getWidth(), static_cast(_bb.getRight() + 1))); + _fontBuffer.resize(std::max(_fontBuffer.getHeight(), static_cast(_bb.getBottom() + 1)), + std::max(_fontBuffer.getWidth(), static_cast(_bb.getRight() + 1))); int wordUTF32_size = static_cast(_wordUTF32.size()); diff --git a/modules/core/src/image/vpImageCircle.cpp b/modules/core/src/image/vpImageCircle.cpp index e63c0545fa..39db6b75d9 100644 --- a/modules/core/src/image/vpImageCircle.cpp +++ b/modules/core/src/image/vpImageCircle.cpp @@ -74,8 +74,8 @@ void computeIntersectionsLeftBorderOnly(const float &u_c, const float &umin_roi, 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); + 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()) { delta_theta = 0.f; @@ -99,8 +99,8 @@ void computeIntersectionsRightBorderOnly(const float &u_c, const float &umax_roi 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); + 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 = 0.f; @@ -131,8 +131,8 @@ void computeIntersectionsTopBorderOnly(const float &v_c, const float &vmin_roi, else { theta2 = -theta1 - M_PIf; } - float theta_min = std::min(theta1, theta2); - float theta_max = std::max(theta1, theta2); + float theta_min = std::min(theta1, theta2); + float theta_max = std::max(theta1, theta2); 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 @@ -173,8 +173,8 @@ void computeIntersectionsBottomBorderOnly(const float &v_c, const float &vmax_ro else { theta2 = -theta1 - M_PIf; } - float theta_min = std::min(theta1, theta2); - float theta_max = std::max(theta1, theta2); + float theta_min = std::min(theta1, theta2); + float theta_max = std::max(theta1, theta2); 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 diff --git a/modules/core/src/image/vpImageConvert.cpp b/modules/core/src/image/vpImageConvert.cpp index 7e12341789..0d4f1e2a62 100644 --- a/modules/core/src/image/vpImageConvert.cpp +++ b/modules/core/src/image/vpImageConvert.cpp @@ -4392,12 +4392,12 @@ void vpImageConvert::RGB2HSV(const unsigned char *rgb, double *hue, double *satu blue = rgb[i * step + 2] / 255.0; if (red > green) { - max = ((std::max))(red, blue); - min = ((std::min))(green, blue); + max = std::max(red, blue); + min = std::min(green, blue); } else { - max = ((std::max))(green, blue); - min = ((std::min))(red, blue); + max = std::max(green, blue); + min = std::min(red, blue); } v = max; diff --git a/modules/core/src/image/vpImageFilter.cpp b/modules/core/src/image/vpImageFilter.cpp index 52ede64e8a..3d76af9339 100644 --- a/modules/core/src/image/vpImageFilter.cpp +++ b/modules/core/src/image/vpImageFilter.cpp @@ -712,7 +712,7 @@ float vpImageFilter::computeCannyThreshold(const cv::Mat &cv_I, const cv::Mat *p break; } } - float upperThresh = std::max(bon, 1.f); + float upperThresh = std::max(bon, 1.f); lowerThresh = lowerThresholdRatio * bon; return upperThresh; } diff --git a/modules/core/src/image/vpImageTools.cpp b/modules/core/src/image/vpImageTools.cpp index 459de84589..8173926f09 100644 --- a/modules/core/src/image/vpImageTools.cpp +++ b/modules/core/src/image/vpImageTools.cpp @@ -149,7 +149,7 @@ void vpImageTools::imageDifference(const vpImage &I1, const vpIma #else for (unsigned int i = 0; i < I1.getSize(); ++i) { int diff = I1.bitmap[i] - I2.bitmap[i] + 128; - Idiff.bitmap[i] = static_cast(std::max(std::min(diff, 255), 0)); + Idiff.bitmap[i] = static_cast(std::max(std::min(diff, 255), 0)); } #endif } diff --git a/modules/core/src/math/matrix/vpColVector.cpp b/modules/core/src/math/matrix/vpColVector.cpp index aae6a60a14..0925573260 100644 --- a/modules/core/src/math/matrix/vpColVector.cpp +++ b/modules/core/src/math/matrix/vpColVector.cpp @@ -775,7 +775,7 @@ int vpColVector::print(std::ostream &s, unsigned int length, char const *intro) // increase totalLength according to maxBefore totalLength = vpMath::maximum(totalLength, maxBefore); // decrease maxAfter according to totalLength - maxAfter = (std::min)(maxAfter, totalLength - maxBefore); + maxAfter = std::min(maxAfter, totalLength - maxBefore); if (maxAfter == 1) maxAfter = 0; diff --git a/modules/core/src/math/matrix/vpMatrix.cpp b/modules/core/src/math/matrix/vpMatrix.cpp index 1183539505..11813f8e65 100644 --- a/modules/core/src/math/matrix/vpMatrix.cpp +++ b/modules/core/src/math/matrix/vpMatrix.cpp @@ -5364,7 +5364,7 @@ V ^\top}_ { n\times n } \f] \f[ */ vpColVector vpMatrix::getDiag() const { - unsigned int min_size = std::min(rowNum, colNum); + unsigned int min_size = std::min(rowNum, colNum); vpColVector diag; if (min_size > 0) { @@ -5688,7 +5688,7 @@ V ^\top}_ { n\times n } \f] \f[ // increase totalLength according to maxBefore totalLength = vpMath::maximum(totalLength, maxBefore); // decrease maxAfter according to totalLength - maxAfter = (std::min)(maxAfter, totalLength - maxBefore); + maxAfter = std::min(maxAfter, totalLength - maxBefore); if (!intro.empty()) s << intro; @@ -6776,10 +6776,10 @@ V ^\top}_ { n\times n } \f] \f[ M.svd(w, v); double max = w[0]; - unsigned int maxRank = std::min(this->getCols(), this->getRows()); + unsigned int maxRank = std::min(this->getCols(), this->getRows()); // The maximum reachable rank is either the number of columns or the number of rows // of the matrix. - unsigned int boundary = std::min(maxRank, w.size()); + unsigned int boundary = std::min(maxRank, w.size()); // boundary is here to ensure that the number of singular values used for the com- // 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 diff --git a/modules/core/src/math/matrix/vpMatrix_qr.cpp b/modules/core/src/math/matrix/vpMatrix_qr.cpp index fafaa4e23a..fcc5e9b97e 100644 --- a/modules/core/src/math/matrix/vpMatrix_qr.cpp +++ b/modules/core/src/math/matrix/vpMatrix_qr.cpp @@ -160,7 +160,7 @@ vpMatrix vpMatrix::inverseByQRLapack() const gsl_A = gsl_matrix_alloc(rowNum, colNum); gsl_Q = gsl_matrix_alloc(rowNum, rowNum); // M by M gsl_R = gsl_matrix_alloc(rowNum, colNum); // M by N - gsl_tau = gsl_vector_alloc(std::min(rowNum, colNum)); + gsl_tau = gsl_vector_alloc(std::min(rowNum, colNum)); gsl_inv.size1 = inv.rowNum; gsl_inv.size2 = inv.colNum; @@ -224,7 +224,7 @@ vpMatrix vpMatrix::inverseByQRLapack() const integer rowNum_ = (integer)this->getRows(); integer colNum_ = (integer)this->getCols(); integer lda = (integer)rowNum_; // lda is the number of rows because we don't use a submatrix - integer dimTau = (std::min)(rowNum_, colNum_); + integer dimTau = std::min(rowNum_, colNum_); integer dimWork = -1; double *tau = new double[dimTau]; double *work = new double[1]; @@ -294,9 +294,9 @@ vpMatrix vpMatrix::inverseByQRLapack() const std::cout << "dtrtri_:" << -info << "th element had an illegal value" << std::endl; else if (info > 0) { std::cout << "dtrtri_:R(" << info << "," << info << ")" - << " is exactly zero. The triangular matrix is singular " - "and its inverse can not be computed." - << std::endl; + << " is exactly zero. The triangular matrix is singular " + "and its inverse can not be computed." + << std::endl; std::cout << "R=" << std::endl << C << std::endl; } throw vpMatrixException::badValue; @@ -335,7 +335,8 @@ vpMatrix vpMatrix::inverseByQRLapack() const } delete[] tau; delete[] work; - } catch (vpMatrixException &) { + } + catch (vpMatrixException &) { delete[] tau; delete[] work; throw; @@ -446,7 +447,7 @@ unsigned int vpMatrix::qr(vpMatrix &Q, vpMatrix &R, bool full, bool squareR, dou #if defined(VISP_HAVE_GSL) unsigned int m = rowNum; // also rows of Q unsigned int n = colNum; // also columns of R - unsigned int r = std::min(n, m); // a priori non-null rows of R = rank of R + unsigned int r = std::min(n, m); // a priori non-null rows of R = rank of R unsigned int q = r; // columns of Q and rows of R unsigned int na = n; // columns of A @@ -470,7 +471,7 @@ unsigned int vpMatrix::qr(vpMatrix &Q, vpMatrix &R, bool full, bool squareR, dou gsl_A = gsl_matrix_alloc(rowNum, colNum); gsl_R = gsl_matrix_alloc(rowNum, colNum); // M by N - gsl_tau = gsl_vector_alloc(std::min(rowNum, colNum)); + gsl_tau = gsl_vector_alloc(std::min(rowNum, colNum)); // copy input matrix since gsl_linalg_QR_decomp() is destructive unsigned int Atda = static_cast(gsl_A->tda); @@ -497,7 +498,8 @@ unsigned int vpMatrix::qr(vpMatrix &Q, vpMatrix &R, bool full, bool squareR, dou gsl_linalg_QR_unpack(gsl_A, gsl_tau, &gsl_Qfull, gsl_R); // std::cout << "gsl_Qfull:\n "; display_gsl(&gsl_Qfull); // std::cout << "gsl_R:\n "; display_gsl(gsl_R); - } else { + } + else { gsl_Q = gsl_matrix_alloc(rowNum, rowNum); // M by M gsl_linalg_QR_unpack(gsl_A, gsl_tau, gsl_Q, gsl_R); @@ -517,7 +519,7 @@ unsigned int vpMatrix::qr(vpMatrix &Q, vpMatrix &R, bool full, bool squareR, dou } // copy useful part of R and update rank - na = std::min(m, n); + 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++) { @@ -537,7 +539,7 @@ unsigned int vpMatrix::qr(vpMatrix &Q, vpMatrix &R, bool full, bool squareR, dou #else integer m = (integer)rowNum; // also rows of Q integer n = (integer)colNum; // also columns of R - integer r = std::min(n, m); // a priori non-null rows of R = rank of R + integer r = std::min(n, m); // a priori non-null rows of R = rank of R integer q = r; // columns of Q and rows of R integer na = n; // columns of A @@ -558,7 +560,7 @@ unsigned int vpMatrix::qr(vpMatrix &Q, vpMatrix &R, bool full, bool squareR, dou integer dimWork = -1; double *qrdata = new double[m * na]; - double *tau = new double[std::min(m, q)]; + double *tau = new double[std::min(m, q)]; double *work = new double[1]; integer info; @@ -610,7 +612,7 @@ unsigned int vpMatrix::qr(vpMatrix &Q, vpMatrix &R, bool full, bool squareR, dou // data now contains the R matrix in its upper triangular (in lapack convention) // copy useful part of R from Q and update rank - na = std::min(m, n); + na = std::min(m, n); if (squareR) { for (int i = 0; i < na; i++) { for (int j = i; j < na; j++) @@ -618,7 +620,8 @@ unsigned int vpMatrix::qr(vpMatrix &Q, vpMatrix &R, bool full, bool squareR, dou if (std::abs(qrdata[i + m * i]) < tol) r--; } - } else { + } + else { for (int i = 0; i < na; i++) { for (int j = i; j < n; j++) R[i][j] = qrdata[i + m * j]; @@ -726,7 +729,7 @@ unsigned int vpMatrix::qrPivot(vpMatrix &Q, vpMatrix &R, vpMatrix &P, bool full, #if defined(VISP_HAVE_GSL) unsigned int m = rowNum; // also rows of Q unsigned int n = colNum; // also columns of R - unsigned int r = std::min(n, m); // a priori non-null rows of R = rank of R + unsigned int r = std::min(n, m); // a priori non-null rows of R = rank of R unsigned int q = r; // columns of Q and rows of R unsigned int na = n; // columns of A @@ -742,7 +745,8 @@ unsigned int vpMatrix::qrPivot(vpMatrix &Q, vpMatrix &R, vpMatrix &P, bool full, if (squareR) { R.resize(0, 0, false); P.resize(0, n, false); - } else { + } + else { R.resize(r, n, false); P.resize(n, n); } @@ -763,7 +767,7 @@ unsigned int vpMatrix::qrPivot(vpMatrix &Q, vpMatrix &R, vpMatrix &P, bool full, gsl_A.block = 0; gsl_R = gsl_matrix_alloc(rowNum, colNum); // M by N - gsl_tau = gsl_vector_alloc(std::min(rowNum, colNum)); + gsl_tau = gsl_vector_alloc(std::min(rowNum, colNum)); gsl_norm = gsl_vector_alloc(colNum); gsl_p = gsl_permutation_alloc(n); @@ -780,7 +784,8 @@ unsigned int vpMatrix::qrPivot(vpMatrix &Q, vpMatrix &R, vpMatrix &P, bool full, gsl_linalg_QRPT_decomp2(&gsl_A, &gsl_Qfull, gsl_R, gsl_tau, gsl_p, &gsl_signum, gsl_norm); // std::cout << "gsl_Qfull:\n "; display_gsl(&gsl_Qfull); // std::cout << "gsl_R:\n "; display_gsl(gsl_R); - } else { + } + else { gsl_Q = gsl_matrix_alloc(rowNum, rowNum); // M by M gsl_linalg_QRPT_decomp2(&gsl_A, gsl_Q, gsl_R, gsl_tau, gsl_p, &gsl_signum, gsl_norm); @@ -800,7 +805,7 @@ unsigned int vpMatrix::qrPivot(vpMatrix &Q, vpMatrix &R, vpMatrix &P, bool full, } // update rank - na = std::min(m, n); + na = std::min(m, n); unsigned int Rtda = static_cast(gsl_R->tda); for (unsigned int i = 0; i < na; i++) { unsigned int k = i * Rtda; @@ -813,7 +818,8 @@ unsigned int vpMatrix::qrPivot(vpMatrix &Q, vpMatrix &R, vpMatrix &P, bool full, P.resize(r, n); for (unsigned int i = 0; i < r; ++i) P[i][gsl_p->data[i]] = 1; - } else { + } + else { R.resize(na, n, false); // R is min(m,n) x n of rank r P.resize(n, n); for (unsigned int i = 0; i < n; ++i) @@ -836,7 +842,7 @@ unsigned int vpMatrix::qrPivot(vpMatrix &Q, vpMatrix &R, vpMatrix &P, bool full, #else integer m = (integer)rowNum; // also rows of Q integer n = (integer)colNum; // also columns of R - integer r = std::min(n, m); // a priori non-null rows of R = rank of R + integer r = >(n, m); // a priori non-null rows of R = rank of R integer q = r; // columns of Q and rows of R integer na = n; // columns of A @@ -853,7 +859,8 @@ unsigned int vpMatrix::qrPivot(vpMatrix &Q, vpMatrix &R, vpMatrix &P, bool full, if (squareR) { R.resize(0, 0); P.resize(0, static_cast(n)); - } else { + } + else { R.resize(static_cast(r), static_cast(n)); P.resize(static_cast(n), static_cast(n)); } @@ -925,7 +932,7 @@ unsigned int vpMatrix::qrPivot(vpMatrix &Q, vpMatrix &R, vpMatrix &P, bool full, // data now contains the R matrix in its upper triangular (in lapack convention) // get rank of R in r - na = std::min(n, m); + na = std::min(n, m); for (int i = 0; i < na; ++i) if (std::abs(qrdata[i + m * i]) < tol) r--; @@ -942,7 +949,8 @@ unsigned int vpMatrix::qrPivot(vpMatrix &Q, vpMatrix &R, vpMatrix &P, bool full, P.resize(static_cast(r), static_cast(n)); for (int i = 0; i < r; ++i) P[i][p[i] - 1] = 1; - } else // R is min(m,n) x n of rank r + } + 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++) @@ -1044,7 +1052,8 @@ vpMatrix vpMatrix::inverseTriangular(bool upper) const } } #endif - } else { + } + else { #if (GSL_MAJOR_VERSION >= 2 && GSL_MINOR_VERSION >= 2) gsl_linalg_tri_lower_invert(&gsl_inv); #else @@ -1085,9 +1094,9 @@ vpMatrix vpMatrix::inverseTriangular(bool upper) const std::cout << "dtrtri_:" << -info << "th element had an illegal value" << std::endl; else if (info > 0) { std::cout << "dtrtri_:R(" << info << "," << info << ")" - << " is exactly zero. The triangular matrix is singular " - "and its inverse can not be computed." - << std::endl; + << " is exactly zero. The triangular matrix is singular " + "and its inverse can not be computed." + << std::endl; throw vpMatrixException::rankDeficient; } throw vpMatrixException::badValue; diff --git a/modules/core/src/math/matrix/vpMatrix_svd.cpp b/modules/core/src/math/matrix/vpMatrix_svd.cpp index 1061ab81a8..094ee588e3 100644 --- a/modules/core/src/math/matrix/vpMatrix_svd.cpp +++ b/modules/core/src/math/matrix/vpMatrix_svd.cpp @@ -247,7 +247,8 @@ void vpMatrix::svdLapack(vpColVector &w, vpMatrix &V) U = this->transpose(); nc = getRows(); nr = getCols(); - } else { + } + else { nc = getCols(); nr = getRows(); } @@ -263,7 +264,8 @@ void vpMatrix::svdLapack(vpColVector &w, vpMatrix &V) A.tda = A.size2; if (rowNum < colNum) { A.data = U.data; - } else { + } + else { A.data = this->data; } A.owner = 0; @@ -302,13 +304,13 @@ void vpMatrix::svdLapack(vpColVector &w, vpMatrix &V) integer n = (integer)(this->getRows()); integer lda = m; integer ldu = m; - integer ldvt = (std::min)(m, n); + integer ldvt = std::min(m, n); integer info, lwork; double wkopt; double *work; - integer *iwork = new integer[8 * static_cast((std::min)(n, m))]; + integer *iwork = new integer[8 * static_cast(std::min(n, m))]; double *s = w.data; double *a = new double[static_cast(lda * n)]; diff --git a/modules/core/src/math/matrix/vpRowVector.cpp b/modules/core/src/math/matrix/vpRowVector.cpp index 54cfa3af73..080859de62 100644 --- a/modules/core/src/math/matrix/vpRowVector.cpp +++ b/modules/core/src/math/matrix/vpRowVector.cpp @@ -998,7 +998,7 @@ int vpRowVector::print(std::ostream &s, unsigned int length, char const *intro) // increase totalLength according to maxBefore totalLength = vpMath::maximum(totalLength, maxBefore); // decrease maxAfter according to totalLength - maxAfter = (std::min)(maxAfter, totalLength - maxBefore); + maxAfter = std::min(maxAfter, totalLength - maxBefore); if (maxAfter == 1) maxAfter = 0; diff --git a/modules/core/src/math/transformation/vpForceTwistMatrix.cpp b/modules/core/src/math/transformation/vpForceTwistMatrix.cpp index 826f1a9c2c..db6fb4a6b6 100644 --- a/modules/core/src/math/transformation/vpForceTwistMatrix.cpp +++ b/modules/core/src/math/transformation/vpForceTwistMatrix.cpp @@ -577,7 +577,8 @@ int vpForceTwistMatrix::print(std::ostream &s, unsigned int length, char const * if (p == std::string::npos) { maxBefore = vpMath::maximum(maxBefore, thislen); // maxAfter remains the same - } else { + } + else { maxBefore = vpMath::maximum(maxBefore, p); maxAfter = vpMath::maximum(maxAfter, thislen - p - 1); } @@ -588,7 +589,7 @@ int vpForceTwistMatrix::print(std::ostream &s, unsigned int length, char const * // increase totalLength according to maxBefore totalLength = vpMath::maximum(totalLength, maxBefore); // decrease maxAfter according to totalLength - maxAfter = (std::min)(maxAfter, totalLength - maxBefore); + maxAfter = std::min(maxAfter, totalLength - maxBefore); if (maxAfter == 1) maxAfter = 0; @@ -612,7 +613,8 @@ int vpForceTwistMatrix::print(std::ostream &s, unsigned int length, char const * if (p != std::string::npos) { s.width((std::streamsize)maxAfter); s << values[i * n + j].substr(p, maxAfter).c_str(); - } else { + } + else { assert(maxAfter > 1); s.width((std::streamsize)maxAfter); s << ".0"; diff --git a/modules/core/src/math/transformation/vpPoseVector.cpp b/modules/core/src/math/transformation/vpPoseVector.cpp index 44e821c27c..9d7ef1f47b 100644 --- a/modules/core/src/math/transformation/vpPoseVector.cpp +++ b/modules/core/src/math/transformation/vpPoseVector.cpp @@ -459,7 +459,7 @@ int vpPoseVector::print(std::ostream &s, unsigned int length, char const *intro) // increase totalLength according to maxBefore totalLength = vpMath::maximum(totalLength, maxBefore); // decrease maxAfter according to totalLength - maxAfter = (std::min)(maxAfter, totalLength - maxBefore); + maxAfter = std::min(maxAfter, totalLength - maxBefore); if (maxAfter == 1) maxAfter = 0; diff --git a/modules/core/src/math/transformation/vpVelocityTwistMatrix.cpp b/modules/core/src/math/transformation/vpVelocityTwistMatrix.cpp index 43cedfcc8a..fddb007238 100644 --- a/modules/core/src/math/transformation/vpVelocityTwistMatrix.cpp +++ b/modules/core/src/math/transformation/vpVelocityTwistMatrix.cpp @@ -537,7 +537,8 @@ int vpVelocityTwistMatrix::print(std::ostream &s, unsigned int length, char cons if (p == std::string::npos) { maxBefore = vpMath::maximum(maxBefore, thislen); // maxAfter remains the same - } else { + } + else { maxBefore = vpMath::maximum(maxBefore, p); maxAfter = vpMath::maximum(maxAfter, thislen - p - 1); } @@ -548,7 +549,7 @@ int vpVelocityTwistMatrix::print(std::ostream &s, unsigned int length, char cons // increase totalLength according to maxBefore totalLength = vpMath::maximum(totalLength, maxBefore); // decrease maxAfter according to totalLength - maxAfter = (std::min)(maxAfter, totalLength - maxBefore); + maxAfter = std::min(maxAfter, totalLength - maxBefore); if (maxAfter == 1) maxAfter = 0; @@ -572,7 +573,8 @@ int vpVelocityTwistMatrix::print(std::ostream &s, unsigned int length, char cons if (p != std::string::npos) { s.width((std::streamsize)maxAfter); s << values[i * n + j].substr(p, maxAfter).c_str(); - } else { + } + else { assert(maxAfter > 1); s.width((std::streamsize)maxAfter); s << ".0"; diff --git a/modules/core/src/tools/file/vpIoTools.cpp b/modules/core/src/tools/file/vpIoTools.cpp index fd192f1c26..bda66bdf19 100644 --- a/modules/core/src/tools/file/vpIoTools.cpp +++ b/modules/core/src/tools/file/vpIoTools.cpp @@ -1530,7 +1530,7 @@ std::string vpIoTools::getFileExtension(const std::string &pathname, bool checkF int sepIndex = static_cast(pathname.rfind(sep)); if (!altsep.empty()) { int altsepIndex = static_cast(pathname.rfind(altsep)); - sepIndex = ((std::max))(sepIndex, altsepIndex); + sepIndex = std::max(sepIndex, altsepIndex); } size_t dotIndex = pathname.rfind(extsep); diff --git a/modules/core/src/tools/optimization/vpLinProg.cpp b/modules/core/src/tools/optimization/vpLinProg.cpp index 0510854830..a3c5e698fc 100644 --- a/modules/core/src/tools/optimization/vpLinProg.cpp +++ b/modules/core/src/tools/optimization/vpLinProg.cpp @@ -429,8 +429,8 @@ bool vpLinProg::solveLP(const vpColVector &c, vpMatrix A, vpColVector b, const v for (unsigned int j = 0; j < m + p; ++j) A[j][n + p + k1] = -A[j][i]; if (feasible) { - x[i] = std::max(x[i], 0.); - x[n + p + k1] = std::max(-x[i], 0.); + x[i] = std::max(x[i], 0.); + x[n + p + k1] = std::max(-x[i], 0.); } k1++; } diff --git a/modules/core/test/image-with-dataset/common.hpp b/modules/core/test/image-with-dataset/common.hpp index a669da0ec2..a10efa2d21 100644 --- a/modules/core/test/image-with-dataset/common.hpp +++ b/modules/core/test/image-with-dataset/common.hpp @@ -128,8 +128,8 @@ unsigned char getPixelClamped(const vpImage &I, float x, float y) { int j = vpMath::round(x); int i = vpMath::round(y); - j = (std::max)(0, (std::min)(j, static_cast(I.getWidth()) - 1)); - i = (std::max)(0, (std::min)(i, static_cast(I.getHeight()) - 1)); + j = std::max(0, std::min(j, static_cast(I.getWidth()) - 1)); + i = std::max(0, std::min(i, static_cast(I.getHeight()) - 1)); return I[i][j]; } @@ -138,8 +138,8 @@ vpRGBa getPixelClamped(const vpImage &I, float x, float y) { int j = vpMath::round(x); int i = vpMath::round(y); - j = (std::max)(0, (std::min)(j, static_cast(I.getWidth()) - 1)); - i = (std::max)(0, (std::min)(i, static_cast(I.getHeight()) - 1)); + j = std::max(0, std::min(j, static_cast(I.getWidth()) - 1)); + i = std::max(0, std::min(i, static_cast(I.getHeight()) - 1)); return I[i][j]; } @@ -149,11 +149,11 @@ float lerp(float A, float B, float t) { return A * (1.0f - t) + B * t; } void resizeBilinear(const vpImage &I, vpImage &Ires, unsigned int i, unsigned int j, int u0, int v0, float xFrac, float yFrac) { - int u1 = (std::min)(static_cast(I.getWidth()) - 1, u0 + 1); + int u1 = std::min(static_cast(I.getWidth()) - 1, u0 + 1); int v1 = v0; int u2 = u0; - int v2 = (std::min)(static_cast(I.getHeight()) - 1, v0 + 1); + int v2 = std::min(static_cast(I.getHeight()) - 1, v0 + 1); int u3 = u1; int v3 = v2; @@ -169,11 +169,11 @@ void resizeBilinear(const vpImage &I, vpImage &Ire void resizeBilinear(const vpImage &I, vpImage &Ires, unsigned int i, unsigned int j, int u0, int v0, float xFrac, float yFrac) { - int u1 = (std::min)(static_cast(I.getWidth()) - 1, u0 + 1); + int u1 = std::min(static_cast(I.getWidth()) - 1, u0 + 1); int v1 = v0; int u2 = u0; - int v2 = (std::min)(static_cast(I.getHeight()) - 1, v0 + 1); + int v2 = std::min(static_cast(I.getHeight()) - 1, v0 + 1); int u3 = u1; int v3 = v2; @@ -213,10 +213,12 @@ void resizeRef(const vpImage &Isrc, vpImage &Idst, if (method == 0) { // nearest neighbor Idst[i][j] = getPixelClamped(Isrc, u, v); - } else if (method == 1) { // bilinear + } + else if (method == 1) { // bilinear resizeBilinear(Isrc, Idst, i, j, u0, v0, xFrac, yFrac); - } else { // bicubic - // no bicubic ref test for now + } + else { // bicubic + // no bicubic ref test for now } } } @@ -240,10 +242,12 @@ void resizeRef(const vpImage &Isrc, vpImage &Idst, int method) if (method == 0) { // nearest neighbor Idst[i][j] = getPixelClamped(Isrc, u, v); - } else if (method == 1) { // bilinear + } + else if (method == 1) { // bilinear resizeBilinear(Isrc, Idst, i, j, u0, v0, xFrac, yFrac); - } else { // bicubic - // no bicubic ref test for now + } + else { // bicubic + // no bicubic ref test for now } } } @@ -403,7 +407,8 @@ void imageErosionRef(vpImage &I, for (unsigned int j = 0; j < J.getWidth(); j++) { J[i][j] = null_value; } - } else { + } + else { J[i][0] = null_value; memcpy(J[i] + 1, I[i - 1], sizeof(unsigned char) * I.getWidth()); J[i][J.getWidth() - 1] = null_value; @@ -411,7 +416,7 @@ void imageErosionRef(vpImage &I, } if (connexity == vpImageMorphology::CONNEXITY_4) { - unsigned int offset[5] = {1, J.getWidth(), J.getWidth() + 1, J.getWidth() + 2, J.getWidth() * 2 + 1}; + unsigned int offset[5] = { 1, J.getWidth(), J.getWidth() + 1, J.getWidth() + 2, J.getWidth() * 2 + 1 }; for (unsigned int i = 0; i < I.getHeight(); i++) { unsigned char *ptr_curr_J = J.bitmap + i * J.getWidth(); @@ -420,15 +425,16 @@ void imageErosionRef(vpImage &I, for (unsigned int j = 0; j < I.getWidth(); j++) { unsigned char min_value = null_value; for (int k = 0; k < 5; k++) { - min_value = (std::min)(min_value, *(ptr_curr_J + j + offset[k])); + min_value = std::min(min_value, *(ptr_curr_J + j + offset[k])); } *(ptr_curr_I + j) = min_value; } } - } else { - // CONNEXITY_8 - unsigned int offset[9] = {0, + } + else { + // CONNEXITY_8 + unsigned int offset[9] = { 0, 1, 2, J.getWidth(), @@ -436,7 +442,7 @@ void imageErosionRef(vpImage &I, J.getWidth() + 2, J.getWidth() * 2, J.getWidth() * 2 + 1, - J.getWidth() * 2 + 2}; + J.getWidth() * 2 + 2 }; for (unsigned int i = 0; i < I.getHeight(); i++) { unsigned char *ptr_curr_J = J.bitmap + i * J.getWidth(); @@ -445,7 +451,7 @@ void imageErosionRef(vpImage &I, for (unsigned int j = 0; j < I.getWidth(); j++) { unsigned char min_value = null_value; for (int k = 0; k < 9; k++) { - min_value = (std::min)(min_value, *(ptr_curr_J + j + offset[k])); + min_value = std::min(min_value, *(ptr_curr_J + j + offset[k])); } *(ptr_curr_I + j) = min_value; @@ -472,7 +478,8 @@ void imageDilatationRef(vpImage &I, for (unsigned int j = 0; j < J.getWidth(); j++) { J[i][j] = null_value; } - } else { + } + else { J[i][0] = null_value; memcpy(J[i] + 1, I[i - 1], sizeof(unsigned char) * I.getWidth()); J[i][J.getWidth() - 1] = null_value; @@ -480,7 +487,7 @@ void imageDilatationRef(vpImage &I, } if (connexity == vpImageMorphology::CONNEXITY_4) { - unsigned int offset[5] = {1, J.getWidth(), J.getWidth() + 1, J.getWidth() + 2, J.getWidth() * 2 + 1}; + unsigned int offset[5] = { 1, J.getWidth(), J.getWidth() + 1, J.getWidth() + 2, J.getWidth() * 2 + 1 }; for (unsigned int i = 0; i < I.getHeight(); i++) { unsigned char *ptr_curr_J = J.bitmap + i * J.getWidth(); @@ -489,15 +496,16 @@ void imageDilatationRef(vpImage &I, for (unsigned int j = 0; j < I.getWidth(); j++) { unsigned char max_value = null_value; for (int k = 0; k < 5; k++) { - max_value = (std::max)(max_value, *(ptr_curr_J + j + offset[k])); + max_value = std::max(max_value, *(ptr_curr_J + j + offset[k])); } *(ptr_curr_I + j) = max_value; } } - } else { + } + else { // CONNEXITY_8 - unsigned int offset[9] = {0, + unsigned int offset[9] = { 0, 1, 2, J.getWidth(), @@ -505,7 +513,7 @@ void imageDilatationRef(vpImage &I, J.getWidth() + 2, J.getWidth() * 2, J.getWidth() * 2 + 1, - J.getWidth() * 2 + 2}; + J.getWidth() * 2 + 2 }; for (unsigned int i = 0; i < I.getHeight(); i++) { unsigned char *ptr_curr_J = J.bitmap + i * J.getWidth(); @@ -514,7 +522,7 @@ void imageDilatationRef(vpImage &I, for (unsigned int j = 0; j < I.getWidth(); j++) { unsigned char max_value = null_value; for (int k = 0; k < 9; k++) { - max_value = (std::max)(max_value, *(ptr_curr_J + j + offset[k])); + max_value = std::max(max_value, *(ptr_curr_J + j + offset[k])); } *(ptr_curr_I + j) = max_value; @@ -538,7 +546,8 @@ void magicSquare(vpImage &magic_square, int N) if (magic_square[newi][newj]) { i++; - } else { + } + else { i = newi; j = newj; } @@ -621,7 +630,8 @@ void fill(cv::Mat &img) for (int j = 0; j < img.cols; j++) { if (img.type() == CV_8UC1) { img.at(i, j) = static_cast(i * img.cols + j); - } else if (img.type() == CV_8UC3) { + } + else if (img.type() == CV_8UC3) { img.at(i, j)[0] = static_cast((i * img.cols + j) * 3); img.at(i, j)[1] = static_cast((i * img.cols + j) * 3 + 1); img.at(i, j)[2] = static_cast((i * img.cols + j) * 3 + 2); diff --git a/modules/core/test/image/common.hpp b/modules/core/test/image/common.hpp index a669da0ec2..5d1f424fe6 100644 --- a/modules/core/test/image/common.hpp +++ b/modules/core/test/image/common.hpp @@ -128,8 +128,8 @@ unsigned char getPixelClamped(const vpImage &I, float x, float y) { int j = vpMath::round(x); int i = vpMath::round(y); - j = (std::max)(0, (std::min)(j, static_cast(I.getWidth()) - 1)); - i = (std::max)(0, (std::min)(i, static_cast(I.getHeight()) - 1)); + j = std::max(0, std::min(j, static_cast(I.getWidth()) - 1)); + i = std::max(0, std::min(i, static_cast(I.getHeight()) - 1)); return I[i][j]; } @@ -138,8 +138,8 @@ vpRGBa getPixelClamped(const vpImage &I, float x, float y) { int j = vpMath::round(x); int i = vpMath::round(y); - j = (std::max)(0, (std::min)(j, static_cast(I.getWidth()) - 1)); - i = (std::max)(0, (std::min)(i, static_cast(I.getHeight()) - 1)); + j = std::max(0, std::min(j, static_cast(I.getWidth()) - 1)); + i = std::max(0, std::min(i, static_cast(I.getHeight()) - 1)); return I[i][j]; } @@ -149,11 +149,11 @@ float lerp(float A, float B, float t) { return A * (1.0f - t) + B * t; } void resizeBilinear(const vpImage &I, vpImage &Ires, unsigned int i, unsigned int j, int u0, int v0, float xFrac, float yFrac) { - int u1 = (std::min)(static_cast(I.getWidth()) - 1, u0 + 1); + int u1 = std::min(static_cast(I.getWidth()) - 1, u0 + 1); int v1 = v0; int u2 = u0; - int v2 = (std::min)(static_cast(I.getHeight()) - 1, v0 + 1); + int v2 = std::min(static_cast(I.getHeight()) - 1, v0 + 1); int u3 = u1; int v3 = v2; @@ -169,11 +169,11 @@ void resizeBilinear(const vpImage &I, vpImage &Ire void resizeBilinear(const vpImage &I, vpImage &Ires, unsigned int i, unsigned int j, int u0, int v0, float xFrac, float yFrac) { - int u1 = (std::min)(static_cast(I.getWidth()) - 1, u0 + 1); + int u1 = std::min(static_cast(I.getWidth()) - 1, u0 + 1); int v1 = v0; int u2 = u0; - int v2 = (std::min)(static_cast(I.getHeight()) - 1, v0 + 1); + int v2 = std::min(static_cast(I.getHeight()) - 1, v0 + 1); int u3 = u1; int v3 = v2; @@ -213,10 +213,12 @@ void resizeRef(const vpImage &Isrc, vpImage &Idst, if (method == 0) { // nearest neighbor Idst[i][j] = getPixelClamped(Isrc, u, v); - } else if (method == 1) { // bilinear + } + else if (method == 1) { // bilinear resizeBilinear(Isrc, Idst, i, j, u0, v0, xFrac, yFrac); - } else { // bicubic - // no bicubic ref test for now + } + else { // bicubic + // no bicubic ref test for now } } } @@ -240,10 +242,12 @@ void resizeRef(const vpImage &Isrc, vpImage &Idst, int method) if (method == 0) { // nearest neighbor Idst[i][j] = getPixelClamped(Isrc, u, v); - } else if (method == 1) { // bilinear + } + else if (method == 1) { // bilinear resizeBilinear(Isrc, Idst, i, j, u0, v0, xFrac, yFrac); - } else { // bicubic - // no bicubic ref test for now + } + else { // bicubic + // no bicubic ref test for now } } } @@ -403,7 +407,8 @@ void imageErosionRef(vpImage &I, for (unsigned int j = 0; j < J.getWidth(); j++) { J[i][j] = null_value; } - } else { + } + else { J[i][0] = null_value; memcpy(J[i] + 1, I[i - 1], sizeof(unsigned char) * I.getWidth()); J[i][J.getWidth() - 1] = null_value; @@ -411,7 +416,7 @@ void imageErosionRef(vpImage &I, } if (connexity == vpImageMorphology::CONNEXITY_4) { - unsigned int offset[5] = {1, J.getWidth(), J.getWidth() + 1, J.getWidth() + 2, J.getWidth() * 2 + 1}; + unsigned int offset[5] = { 1, J.getWidth(), J.getWidth() + 1, J.getWidth() + 2, J.getWidth() * 2 + 1 }; for (unsigned int i = 0; i < I.getHeight(); i++) { unsigned char *ptr_curr_J = J.bitmap + i * J.getWidth(); @@ -420,15 +425,16 @@ void imageErosionRef(vpImage &I, for (unsigned int j = 0; j < I.getWidth(); j++) { unsigned char min_value = null_value; for (int k = 0; k < 5; k++) { - min_value = (std::min)(min_value, *(ptr_curr_J + j + offset[k])); + min_value = std::min(min_value, *(ptr_curr_J + j + offset[k])); } *(ptr_curr_I + j) = min_value; } } - } else { - // CONNEXITY_8 - unsigned int offset[9] = {0, + } + else { + // CONNEXITY_8 + unsigned int offset[9] = { 0, 1, 2, J.getWidth(), @@ -436,7 +442,7 @@ void imageErosionRef(vpImage &I, J.getWidth() + 2, J.getWidth() * 2, J.getWidth() * 2 + 1, - J.getWidth() * 2 + 2}; + J.getWidth() * 2 + 2 }; for (unsigned int i = 0; i < I.getHeight(); i++) { unsigned char *ptr_curr_J = J.bitmap + i * J.getWidth(); @@ -445,7 +451,7 @@ void imageErosionRef(vpImage &I, for (unsigned int j = 0; j < I.getWidth(); j++) { unsigned char min_value = null_value; for (int k = 0; k < 9; k++) { - min_value = (std::min)(min_value, *(ptr_curr_J + j + offset[k])); + min_value = std::min(min_value, *(ptr_curr_J + j + offset[k])); } *(ptr_curr_I + j) = min_value; @@ -472,7 +478,8 @@ void imageDilatationRef(vpImage &I, for (unsigned int j = 0; j < J.getWidth(); j++) { J[i][j] = null_value; } - } else { + } + else { J[i][0] = null_value; memcpy(J[i] + 1, I[i - 1], sizeof(unsigned char) * I.getWidth()); J[i][J.getWidth() - 1] = null_value; @@ -480,7 +487,7 @@ void imageDilatationRef(vpImage &I, } if (connexity == vpImageMorphology::CONNEXITY_4) { - unsigned int offset[5] = {1, J.getWidth(), J.getWidth() + 1, J.getWidth() + 2, J.getWidth() * 2 + 1}; + unsigned int offset[5] = { 1, J.getWidth(), J.getWidth() + 1, J.getWidth() + 2, J.getWidth() * 2 + 1 }; for (unsigned int i = 0; i < I.getHeight(); i++) { unsigned char *ptr_curr_J = J.bitmap + i * J.getWidth(); @@ -489,15 +496,16 @@ void imageDilatationRef(vpImage &I, for (unsigned int j = 0; j < I.getWidth(); j++) { unsigned char max_value = null_value; for (int k = 0; k < 5; k++) { - max_value = (std::max)(max_value, *(ptr_curr_J + j + offset[k])); + max_value = std::max(max_value, *(ptr_curr_J + j + offset[k])); } *(ptr_curr_I + j) = max_value; } } - } else { - // CONNEXITY_8 - unsigned int offset[9] = {0, + } + else { + // CONNEXITY_8 + unsigned int offset[9] = { 0, 1, 2, J.getWidth(), @@ -505,7 +513,7 @@ void imageDilatationRef(vpImage &I, J.getWidth() + 2, J.getWidth() * 2, J.getWidth() * 2 + 1, - J.getWidth() * 2 + 2}; + J.getWidth() * 2 + 2 }; for (unsigned int i = 0; i < I.getHeight(); i++) { unsigned char *ptr_curr_J = J.bitmap + i * J.getWidth(); @@ -514,7 +522,7 @@ void imageDilatationRef(vpImage &I, for (unsigned int j = 0; j < I.getWidth(); j++) { unsigned char max_value = null_value; for (int k = 0; k < 9; k++) { - max_value = (std::max)(max_value, *(ptr_curr_J + j + offset[k])); + max_value = std::max(max_value, *(ptr_curr_J + j + offset[k])); } *(ptr_curr_I + j) = max_value; @@ -538,7 +546,8 @@ void magicSquare(vpImage &magic_square, int N) if (magic_square[newi][newj]) { i++; - } else { + } + else { i = newi; j = newj; } @@ -621,7 +630,8 @@ void fill(cv::Mat &img) for (int j = 0; j < img.cols; j++) { if (img.type() == CV_8UC1) { img.at(i, j) = static_cast(i * img.cols + j); - } else if (img.type() == CV_8UC3) { + } + else if (img.type() == CV_8UC3) { img.at(i, j)[0] = static_cast((i * img.cols + j) * 3); img.at(i, j)[1] = static_cast((i * img.cols + j) * 3 + 1); img.at(i, j)[2] = static_cast((i * img.cols + j) * 3 + 2); diff --git a/modules/core/test/image/testImageGetValue.cpp b/modules/core/test/image/testImageGetValue.cpp index 50f6436e56..577d24e59a 100644 --- a/modules/core/test/image/testImageGetValue.cpp +++ b/modules/core/test/image/testImageGetValue.cpp @@ -61,8 +61,8 @@ template <> vpRGBa checkPixelAccess(unsigned int height, unsigned int width, dou for (unsigned int i = 0; i < I.getHeight(); i++) { for (unsigned int j = 0; j < I.getWidth(); j++) { I[i][j] = - vpRGBa(static_cast(i * I.getWidth() + j), static_cast(i * I.getWidth() + j), - static_cast(i * I.getWidth() + j)); + vpRGBa(static_cast(i * I.getWidth() + j), static_cast(i * I.getWidth() + j), + static_cast(i * I.getWidth() + j)); } } @@ -101,12 +101,12 @@ template PixelType getValue(const vpImage &I, doubl double rfrac = 1.0 - rratio; double cfrac = 1.0 - cratio; - unsigned int iround_1 = (std::min)(I.getHeight() - 1, iround + 1); - unsigned int jround_1 = (std::min)(I.getWidth() - 1, jround + 1); + unsigned int iround_1 = std::min(I.getHeight() - 1, iround + 1); + unsigned int jround_1 = std::min(I.getWidth() - 1, jround + 1); double value = - (static_cast(I[iround][jround]) * rfrac + static_cast(I[iround_1][jround]) * rratio) * cfrac + - (static_cast(I[iround][jround_1]) * rfrac + static_cast(I[iround_1][jround_1]) * rratio) * cratio; + (static_cast(I[iround][jround]) * rfrac + static_cast(I[iround_1][jround]) * rratio) * cfrac + + (static_cast(I[iround][jround_1]) * rfrac + static_cast(I[iround_1][jround_1]) * rratio) * cratio; return static_cast(roundValue ? vpMath::round(value) : value); } @@ -119,21 +119,23 @@ int main() { // unsigned char std::cout << "checkPixelAccess(3, 4, 2, 3): " - << static_cast(checkPixelAccess(3, 4, 2, 3)) << std::endl; + << static_cast(checkPixelAccess(3, 4, 2, 3)) << std::endl; try { std::cout << "checkPixelAccess(3, 4, -2, -3): " - << static_cast(checkPixelAccess(3, 4, -2, -3)) << std::endl; + << static_cast(checkPixelAccess(3, 4, -2, -3)) << std::endl; std::cerr << "Out of image access exception should have been thrown" << std::endl; return EXIT_FAILURE; - } catch (...) { + } + catch (...) { std::cout << "\n"; } try { std::cout << "checkPixelAccess(3, 4, 3, 4): " - << static_cast(checkPixelAccess(3, 4, 3, 4)) << std::endl; + << static_cast(checkPixelAccess(3, 4, 3, 4)) << std::endl; std::cerr << "Out of image access exception should have been thrown" << std::endl; return EXIT_FAILURE; - } catch (...) { + } + catch (...) { std::cout << "\n"; } @@ -143,14 +145,16 @@ int main() std::cout << "checkPixelAccess(3, 4, -2, -3): " << checkPixelAccess(3, 4, -2, -3) << std::endl; std::cerr << "Out of image access exception should have been thrown" << std::endl; return EXIT_FAILURE; - } catch (...) { + } + catch (...) { std::cout << "\n"; } try { std::cout << "checkPixelAccess(3, 4, 3, 4): " << checkPixelAccess(3, 4, 3, 4) << std::endl; std::cerr << "Out of image access exception should have been thrown" << std::endl; return EXIT_FAILURE; - } catch (...) { + } + catch (...) { std::cout << "\n"; } @@ -160,14 +164,16 @@ int main() std::cout << "checkPixelAccess(3, 4, -2, -3): " << checkPixelAccess(3, 4, -2, -3) << std::endl; std::cerr << "Out of image access exception should have been thrown" << std::endl; return EXIT_FAILURE; - } catch (...) { + } + catch (...) { std::cout << "\n"; } try { std::cout << "checkPixelAccess(3, 4, 3, 4): " << checkPixelAccess(3, 4, 3, 4) << std::endl; std::cerr << "Out of image access exception should have been thrown" << std::endl; return EXIT_FAILURE; - } catch (...) { + } + catch (...) { std::cout << "\n"; } @@ -177,14 +183,16 @@ int main() std::cout << "checkPixelAccess(3, 4, -2, -3): " << checkPixelAccess(3, 4, -2, -3) << std::endl; std::cerr << "Out of image access exception should have been thrown" << std::endl; return EXIT_FAILURE; - } catch (...) { + } + catch (...) { std::cout << "\n"; } try { std::cout << "checkPixelAccess(3, 4, 3, 4): " << checkPixelAccess(3, 4, 3, 4) << std::endl; std::cerr << "Out of image access exception should have been thrown" << std::endl; return EXIT_FAILURE; - } catch (...) { + } + catch (...) { std::cout << "\n"; } } @@ -222,7 +230,7 @@ int main() const double maxInterpolationErrorDiff = 1.0; if (std::fabs(meanDiffRound) > maxInterpolationErrorDiff) { std::cerr << "Too much pixel difference between fixed-point vpImage::getValue(double, double) and old method." - << std::endl; + << std::endl; return EXIT_FAILURE; } } @@ -252,7 +260,7 @@ int main() } t_optim = vpTime::measureTimeMs() - t_optim; std::cout << "\nFixed-point vpImage::getValue(double, double), sum1: " << sum1 << " in " << t_optim << " ms" - << std::endl; + << std::endl; int sum2 = 0; double t_old = vpTime::measureTimeMs(); @@ -291,7 +299,7 @@ int main() } t_optim = vpTime::measureTimeMs() - t_optim; std::cout << "\nFixed-point vpImage::getValue(double, double), sum1: " << sum1 << " in " << t_optim << " ms" - << std::endl; + << std::endl; int sum2 = 0; double t_old = vpTime::measureTimeMs(); @@ -327,7 +335,7 @@ int main() bool same = (I == I_copy); std::cout << "\nCheck that getValue returns correct results for integer coordinates\n(I == I_copy)? " << same - << std::endl; + << std::endl; if (!same) { std::cerr << "Issue with vpImage::getValue(double, double)!" << std::endl; return EXIT_FAILURE; diff --git a/modules/core/test/tools/geometry/testImageCircle.cpp b/modules/core/test/tools/geometry/testImageCircle.cpp index 8b78ae4ae3..757172fc86 100644 --- a/modules/core/test/tools/geometry/testImageCircle.cpp +++ b/modules/core/test/tools/geometry/testImageCircle.cpp @@ -48,7 +48,7 @@ int main() const float OFFSET = 5.f; const float WIDTH = 640.f; const float HEIGHT = 480.f; - const float RADIUS = std::min(WIDTH, HEIGHT) / 10.f; + const float RADIUS = std::min(WIDTH, HEIGHT) / 10.f; vpRect roi(OFFSET, OFFSET, WIDTH, HEIGHT); const float WIDTH_SWITCHED = HEIGHT; // The RoI must be inverted in order to cross left and right axes while crossing only the top axis const float HEIGHT_SWITCHED = WIDTH; // The RoI must be inverted in order to cross left and right axes while crossing only the top axis @@ -541,7 +541,7 @@ int main() float theta_v_min = M_PIf / 4.f; float uc = OFFSET - RADIUS * std::cos(theta_v_min); float vc = OFFSET + RADIUS * std::sin(theta_v_min) + 1.f; - vc = std::max(vc, OFFSET + RADIUS * std::sin(-theta_v_min) + 1.f); + vc = std::max(vc, OFFSET + RADIUS * std::sin(-theta_v_min) + 1.f); vpImageCircle circle(vpImagePoint(vc, uc), RADIUS); float arcLengthCircle = circle.computeArcLengthInRoI(roi); float theoreticalValue = M_PI_2f * RADIUS; @@ -574,7 +574,7 @@ int main() float theta_u_top_min = -1.1f * M_PI_2f; float uc = OFFSET - RADIUS * std::cos(theta_u_top_min) + 1.f; - uc = std::max(uc, OFFSET - RADIUS * std::cos(M_PIf - theta_u_top_min) + 1.f); + uc = std::max(uc, OFFSET - RADIUS * std::cos(M_PIf - theta_u_top_min) + 1.f); float vc = OFFSET + RADIUS * std::sin(theta_u_top_min); vpImageCircle circle(vpImagePoint(vc, uc), RADIUS); float arcLengthCircle = circle.computeArcLengthInRoI(roi); @@ -611,7 +611,7 @@ int main() float theta_u_top_min = 5.f * M_PIf / 8.f; float theta_u_top_max = M_PIf - theta_u_top_min; float uc = OFFSET - RADIUS * std::cos(theta_u_top_min) + 1.f; - uc = std::max(uc, OFFSET - RADIUS * std::cos(M_PIf - theta_u_top_min) + 1.f); + uc = std::max(uc, OFFSET - RADIUS * std::cos(M_PIf - theta_u_top_min) + 1.f); float vc = OFFSET + RADIUS * std::sin(theta_u_top_min); float theta_v_min = std::acos((OFFSET - uc)/RADIUS); theta_v_min = vpMath::getAngleBetweenMinPiAndPi(theta_v_min); @@ -762,7 +762,7 @@ int main() float theta_u_top_min = 5.f * M_PIf / 8.f; float theta_u_top_max = M_PIf - theta_u_top_min; float uc = OFFSET + WIDTH - RADIUS * std::cos(theta_u_top_min) - 1.f; - uc = std::min(uc, OFFSET + WIDTH - RADIUS * std::cos(M_PIf - theta_u_top_min) - 1.f); + uc = std::min(uc, OFFSET + WIDTH - RADIUS * std::cos(M_PIf - theta_u_top_min) - 1.f); float vc = OFFSET + RADIUS * std::sin(theta_u_top_min); float theta_v_min = std::acos((OFFSET + WIDTH - uc)/RADIUS); theta_v_min = vpMath::getAngleBetweenMinPiAndPi(theta_v_min); @@ -841,7 +841,7 @@ int main() float theta_v_min = M_PI_4f / 2.f; float theta_v_max = -theta_v_min; float uc = OFFSET - RADIUS * std::cos(theta_v_min); - float vc = std::min(OFFSET + HEIGHT + RADIUS * std::sin(theta_v_min) - 1.f, OFFSET + HEIGHT + RADIUS * std::sin(theta_v_max) - 1.f); + float vc = std::min(OFFSET + HEIGHT + RADIUS * std::sin(theta_v_min) - 1.f, OFFSET + HEIGHT + RADIUS * std::sin(theta_v_max) - 1.f); vpImageCircle circle(vpImagePoint(vc, uc), RADIUS); float arcLengthCircle = circle.computeArcLengthInRoI(roi); float theoreticalValue = (2.f * theta_v_min) * RADIUS; @@ -877,7 +877,7 @@ int main() float theta_u_bot_min = 5.f * M_PI_4f / 2.f; float theta_u_bot_max = M_PIf - theta_u_bot_min; float vc = OFFSET + HEIGHT + RADIUS * std::sin(theta_u_bot_min); - float uc = std::max(OFFSET - RADIUS * std::cos(theta_u_bot_min) + 1.f, OFFSET - RADIUS * std::cos(theta_u_bot_max) + 1.f); + float uc = std::max(OFFSET - RADIUS * std::cos(theta_u_bot_min) + 1.f, OFFSET - RADIUS * std::cos(theta_u_bot_max) + 1.f); vpImageCircle circle(vpImagePoint(vc, uc), RADIUS); float arcLengthCircle = circle.computeArcLengthInRoI(roi); float theoreticalValue = (theta_u_bot_min - theta_u_bot_max) * RADIUS; @@ -984,7 +984,7 @@ int main() // (2) & (4) theta_v_min = - theta_v_max float theta_v_min = 5.f * M_PIf / 6.f; float uc = OFFSET + WIDTH - RADIUS * std::cos(theta_v_min); - float vc = std::min(OFFSET + HEIGHT + RADIUS * std::sin(theta_v_min) - 1.f, OFFSET + HEIGHT + RADIUS * std::sin(-theta_v_min) - 1.f); + float vc = std::min(OFFSET + HEIGHT + RADIUS * std::sin(theta_v_min) - 1.f, OFFSET + HEIGHT + RADIUS * std::sin(-theta_v_min) - 1.f); vpImageCircle circle(vpImagePoint(vc, uc), RADIUS); float arcLengthCircle = circle.computeArcLengthInRoI(roi); float theoreticalValue = (M_PIf / 3.f) * RADIUS; // <=> 2.f * M_PIf / 6.f @@ -1018,7 +1018,7 @@ int main() // (2) & (4) theta_v_min = - theta_v_max float theta_u_bot_min = 4.f * M_PIf / 6.f; float vc = OFFSET + HEIGHT + RADIUS * std::sin(theta_u_bot_min); - float uc = std::min(OFFSET + WIDTH - RADIUS * std::cos(theta_u_bot_min) - 1.f, OFFSET + WIDTH - RADIUS * std::cos(M_PIf -theta_u_bot_min) - 1.f); + float uc = std::min(OFFSET + WIDTH - RADIUS * std::cos(theta_u_bot_min) - 1.f, OFFSET + WIDTH - RADIUS * std::cos(M_PIf -theta_u_bot_min) - 1.f); vpImageCircle circle(vpImagePoint(vc, uc), RADIUS); float arcLengthCircle = circle.computeArcLengthInRoI(roi); float theoreticalValue = (M_PIf / 3.f) * RADIUS; // <=> 2.f * M_PIf / 6.f diff --git a/modules/gui/src/display/vpDisplayGTK.cpp b/modules/gui/src/display/vpDisplayGTK.cpp index fc7ab4cccc..b070bb9c85 100644 --- a/modules/gui/src/display/vpDisplayGTK.cpp +++ b/modules/gui/src/display/vpDisplayGTK.cpp @@ -911,8 +911,8 @@ void vpDisplayGTK::displayImageROI(const vpImage &I, const vpImag vpImageTools::crop(I, iP.get_i(), iP.get_j(), h, w, Itemp, m_scale, m_scale); /* Copie de l'image dans le pixmap fond */ - int i_min = (std::max)(static_cast(ceil(iP.get_i() / m_scale)), 0); - int j_min = (std::max)(static_cast(ceil(iP.get_j() / m_scale)), 0); + int i_min = std::max(static_cast(ceil(iP.get_i() / m_scale)), 0); + int j_min = std::max(static_cast(ceil(iP.get_j() / m_scale)), 0); m_impl->displayImageROI(Itemp, static_cast(j_min), static_cast(i_min), static_cast(Itemp.getWidth()), static_cast(Itemp.getHeight())); @@ -967,8 +967,8 @@ void vpDisplayGTK::displayImageROI(const vpImage &I, const vpImagePoint vpImageTools::crop(I, iP.get_i(), iP.get_j(), h, w, Itemp, m_scale, m_scale); /* Copie de l'image dans le pixmap fond */ - int i_min = (std::max)(static_cast(ceil(iP.get_i() / m_scale)), 0); - int j_min = (std::max)(static_cast(ceil(iP.get_j() / m_scale)), 0); + int i_min = std::max(static_cast(ceil(iP.get_i() / m_scale)), 0); + int j_min = std::max(static_cast(ceil(iP.get_j() / m_scale)), 0); m_impl->displayImageROI(Itemp, static_cast(j_min), static_cast(i_min), static_cast(Itemp.getWidth()), static_cast(Itemp.getHeight())); diff --git a/modules/gui/src/display/vpDisplayOpenCV.cpp b/modules/gui/src/display/vpDisplayOpenCV.cpp index f5b570e8b5..d0b73d743d 100644 --- a/modules/gui/src/display/vpDisplayOpenCV.cpp +++ b/modules/gui/src/display/vpDisplayOpenCV.cpp @@ -697,8 +697,8 @@ void vpDisplayOpenCV::displayImageROI(const vpImage &I, const vpI if (m_scale == 1) { unsigned int i_min = (unsigned int)iP.get_i(); unsigned int j_min = (unsigned int)iP.get_j(); - unsigned int i_max = (std::min)(i_min + h, m_height); - unsigned int j_max = (std::min)(j_min + w, m_width); + unsigned int i_max = std::min(i_min + h, m_height); + unsigned int j_max = std::min(j_min + w, m_width); for (unsigned int i = i_min; i < i_max; i++) { unsigned char *dst_24 = (unsigned char *)m_background->imageData + (int)(i * m_background->widthStep + j_min * 3); @@ -711,10 +711,10 @@ void vpDisplayOpenCV::displayImageROI(const vpImage &I, const vpI } } else { - int i_min = (std::max)((int)ceil(iP.get_i() / m_scale), 0); - int j_min = (std::max)((int)ceil(iP.get_j() / m_scale), 0); - int i_max = (std::min)((int)ceil((iP.get_i() + h) / m_scale), (int)m_height); - int j_max = (std::min)((int)ceil((iP.get_j() + w) / m_scale), (int)m_width); + int i_min = std::max((int)ceil(iP.get_i() / m_scale), 0); + int j_min = std::max((int)ceil(iP.get_j() / m_scale), 0); + int i_max = std::min((int)ceil((iP.get_i() + h) / m_scale), (int)m_height); + int j_max = std::min((int)ceil((iP.get_j() + w) / m_scale), (int)m_width); for (int i = i_min; i < i_max; i++) { unsigned char *dst_24 = (unsigned char *)m_background->imageData + (int)(i * m_background->widthStep + j_min * 3); @@ -739,8 +739,8 @@ void vpDisplayOpenCV::displayImageROI(const vpImage &I, const vpI if (m_scale == 1) { unsigned int i_min = (unsigned int)iP.get_i(); unsigned int j_min = (unsigned int)iP.get_j(); - unsigned int i_max = (std::min)(i_min + h, m_height); - unsigned int j_max = (std::min)(j_min + w, m_width); + unsigned int i_max = std::min(i_min + h, m_height); + unsigned int j_max = std::min(j_min + w, m_width); for (unsigned int i = i_min; i < i_max; i++) { unsigned char *dst_24 = (unsigned char *)m_background.data + (int)(i * 3 * m_width + j_min * 3); for (unsigned int j = j_min; j < j_max; j++) { @@ -752,10 +752,10 @@ void vpDisplayOpenCV::displayImageROI(const vpImage &I, const vpI } } else { - int i_min = (std::max)((int)ceil(iP.get_i() / m_scale), 0); - int j_min = (std::max)((int)ceil(iP.get_j() / m_scale), 0); - int i_max = (std::min)((int)ceil((iP.get_i() + h) / m_scale), (int)m_height); - int j_max = (std::min)((int)ceil((iP.get_j() + w) / m_scale), (int)m_width); + int i_min = std::max((int)ceil(iP.get_i() / m_scale), 0); + int j_min = std::max((int)ceil(iP.get_j() / m_scale), 0); + int i_max = std::min((int)ceil((iP.get_i() + h) / m_scale), (int)m_height); + int j_max = std::min((int)ceil((iP.get_j() + w) / m_scale), (int)m_width); for (int i = i_min; i < i_max; i++) { unsigned char *dst_24 = (unsigned char *)m_background.data + (int)(i * 3 * m_width + j_min * 3); for (int j = j_min; j < j_max; j++) { @@ -901,8 +901,8 @@ void vpDisplayOpenCV::displayImageROI(const vpImage &I, const vpImagePoi if (m_scale == 1) { unsigned int i_min = (unsigned int)iP.get_i(); unsigned int j_min = (unsigned int)iP.get_j(); - unsigned int i_max = (std::min)(i_min + h, m_height); - unsigned int j_max = (std::min)(j_min + w, m_width); + unsigned int i_max = std::min(i_min + h, m_height); + unsigned int j_max = std::min(j_min + w, m_width); for (unsigned int i = i_min; i < i_max; i++) { unsigned char *dst_24 = (unsigned char *)m_background->imageData + (int)(i * m_background->widthStep + j_min * 3); @@ -915,10 +915,10 @@ void vpDisplayOpenCV::displayImageROI(const vpImage &I, const vpImagePoi } } else { - int i_min = (std::max)((int)ceil(iP.get_i() / m_scale), 0); - int j_min = (std::max)((int)ceil(iP.get_j() / m_scale), 0); - int i_max = (std::min)((int)ceil((iP.get_i() + h) / m_scale), (int)m_height); - int j_max = (std::min)((int)ceil((iP.get_j() + w) / m_scale), (int)m_width); + int i_min = std::max((int)ceil(iP.get_i() / m_scale), 0); + int j_min = std::max((int)ceil(iP.get_j() / m_scale), 0); + int i_max = std::min((int)ceil((iP.get_i() + h) / m_scale), (int)m_height); + int j_max = std::min((int)ceil((iP.get_j() + w) / m_scale), (int)m_width); for (int i = i_min; i < i_max; i++) { unsigned char *dst_24 = (unsigned char *)m_background->imageData + (int)(i * m_background->widthStep + j_min * 3); @@ -942,8 +942,8 @@ void vpDisplayOpenCV::displayImageROI(const vpImage &I, const vpImagePoi if (m_scale == 1) { unsigned int i_min = (unsigned int)iP.get_i(); unsigned int j_min = (unsigned int)iP.get_j(); - unsigned int i_max = (std::min)(i_min + h, m_height); - unsigned int j_max = (std::min)(j_min + w, m_width); + unsigned int i_max = std::min(i_min + h, m_height); + unsigned int j_max = std::min(j_min + w, m_width); for (unsigned int i = i_min; i < i_max; i++) { unsigned char *dst_24 = (unsigned char *)m_background.data + (int)(i * 3 * m_width + j_min * 3); for (unsigned int j = j_min; j < j_max; j++) { @@ -955,10 +955,10 @@ void vpDisplayOpenCV::displayImageROI(const vpImage &I, const vpImagePoi } } else { - int i_min = (std::max)((int)ceil(iP.get_i() / m_scale), 0); - int j_min = (std::max)((int)ceil(iP.get_j() / m_scale), 0); - int i_max = (std::min)((int)ceil((iP.get_i() + h) / m_scale), (int)m_height); - int j_max = (std::min)((int)ceil((iP.get_j() + w) / m_scale), (int)m_width); + int i_min = std::max((int)ceil(iP.get_i() / m_scale), 0); + int j_min = std::max((int)ceil(iP.get_j() / m_scale), 0); + int i_max = std::min((int)ceil((iP.get_i() + h) / m_scale), (int)m_height); + int j_max = std::min((int)ceil((iP.get_j() + w) / m_scale), (int)m_width); for (int i = i_min; i < i_max; i++) { unsigned char *dst_24 = (unsigned char *)m_background.data + (int)(i * 3 * m_width + j_min * 3); for (int j = j_min; j < j_max; j++) { diff --git a/modules/gui/src/display/vpDisplayX.cpp b/modules/gui/src/display/vpDisplayX.cpp index b49df221ed..bc9ec6d092 100644 --- a/modules/gui/src/display/vpDisplayX.cpp +++ b/modules/gui/src/display/vpDisplayX.cpp @@ -485,12 +485,12 @@ class vpDisplayX::Impl w, h); } else { - // Correction de l'image de facon a liberer les niveaux de gris - // ROUGE, VERT, BLEU, JAUNE - int i_min = (std::max)((int)ceil(iP.get_i() / scale), 0); - int j_min = (std::max)((int)ceil(iP.get_j() / scale), 0); - int i_max = (std::min)((int)ceil((iP.get_i() + h) / scale), (int)height); - int j_max = (std::min)((int)ceil((iP.get_j() + w) / scale), (int)width); + // Correction de l'image de facon a liberer les niveaux de gris + // ROUGE, VERT, BLEU, JAUNE + int i_min = std::max((int)ceil(iP.get_i() / scale), 0); + int j_min = std::max((int)ceil(iP.get_j() / scale), 0); + int i_max = std::min((int)ceil((iP.get_i() + h) / scale), (int)height); + int j_max = std::min((int)ceil((iP.get_j() + w) / scale), (int)width); unsigned int i_min_ = (unsigned int)i_min; unsigned int i_max_ = (unsigned int)i_max; @@ -529,10 +529,10 @@ class vpDisplayX::Impl w, h); } else { - int i_min = (std::max)((int)ceil(iP.get_i() / scale), 0); - int j_min = (std::max)((int)ceil(iP.get_j() / scale), 0); - int i_max = (std::min)((int)ceil((iP.get_i() + h) / scale), (int)height); - int j_max = (std::min)((int)ceil((iP.get_j() + w) / scale), (int)width); + int i_min = std::max((int)ceil(iP.get_i() / scale), 0); + int j_min = std::max((int)ceil(iP.get_j() / scale), 0); + int i_max = std::min((int)ceil((iP.get_i() + h) / scale), (int)height); + int j_max = std::min((int)ceil((iP.get_j() + w) / scale), (int)width); unsigned int i_min_ = (unsigned int)i_min; unsigned int i_max_ = (unsigned int)i_max; @@ -602,10 +602,10 @@ class vpDisplayX::Impl w, h); } else { - int i_min = (std::max)((int)ceil(iP.get_i() / scale), 0); - int j_min = (std::max)((int)ceil(iP.get_j() / scale), 0); - int i_max = (std::min)((int)ceil((iP.get_i() + h) / scale), (int)height); - int j_max = (std::min)((int)ceil((iP.get_j() + w) / scale), (int)width); + int i_min = std::max((int)ceil(iP.get_i() / scale), 0); + int j_min = std::max((int)ceil(iP.get_j() / scale), 0); + int i_max = std::min((int)ceil((iP.get_i() + h) / scale), (int)height); + int j_max = std::min((int)ceil((iP.get_j() + w) / scale), (int)width); unsigned int i_min_ = (unsigned int)i_min; unsigned int i_max_ = (unsigned int)i_max; @@ -672,10 +672,10 @@ class vpDisplayX::Impl } else { unsigned int bytes_per_line = (unsigned int)Ximage->bytes_per_line; - int i_min = (std::max)((int)ceil(iP.get_i() / scale), 0); - int j_min = (std::max)((int)ceil(iP.get_j() / scale), 0); - int i_max = (std::min)((int)ceil((iP.get_i() + h) / scale), (int)height); - int j_max = (std::min)((int)ceil((iP.get_j() + w) / scale), (int)width); + int i_min = std::max((int)ceil(iP.get_i() / scale), 0); + int j_min = std::max((int)ceil(iP.get_j() / scale), 0); + int i_max = std::min((int)ceil((iP.get_i() + h) / scale), (int)height); + int j_max = std::min((int)ceil((iP.get_j() + w) / scale), (int)width); unsigned int i_min_ = (unsigned int)i_min; unsigned int i_max_ = (unsigned int)i_max; @@ -758,10 +758,10 @@ class vpDisplayX::Impl w, h); } else { - int i_min = (std::max)((int)ceil(iP.get_i() / scale), 0); - int j_min = (std::max)((int)ceil(iP.get_j() / scale), 0); - int i_max = (std::min)((int)ceil((iP.get_i() + h) / scale), (int)height); - int j_max = (std::min)((int)ceil((iP.get_j() + w) / scale), (int)width); + int i_min = std::max((int)ceil(iP.get_i() / scale), 0); + int j_min = std::max((int)ceil(iP.get_j() / scale), 0); + int i_max = std::min((int)ceil((iP.get_i() + h) / scale), (int)height); + int j_max = std::min((int)ceil((iP.get_j() + w) / scale), (int)width); unsigned int i_min_ = (unsigned int)i_min; unsigned int i_max_ = (unsigned int)i_max; diff --git a/modules/gui/src/display/windows/vpD3DRenderer.cpp b/modules/gui/src/display/windows/vpD3DRenderer.cpp index fe8357fe23..e2ce40fae9 100644 --- a/modules/gui/src/display/windows/vpD3DRenderer.cpp +++ b/modules/gui/src/display/windows/vpD3DRenderer.cpp @@ -298,7 +298,8 @@ void vpD3DRenderer::convert(const vpImage &I, unsigned char *imBuffer, u imBuffer[++index_] = val.A; } } - } else { + } + else { for (unsigned int i = 0; i < m_rheight; i++) { unsigned int i_ = i * m_rscale; unsigned int ii_ = i * pitch; @@ -334,7 +335,8 @@ void vpD3DRenderer::convert(const vpImage &I, unsigned char *imBu imBuffer[++index_] = vpRGBa::alpha_default; } } - } else { + } + else { for (unsigned int i = 0; i < m_rheight; i++) { unsigned int i_ = i * m_rscale; unsigned int ii_ = i * pitch; @@ -377,7 +379,8 @@ void vpD3DRenderer::convertROI(const vpImage &I, unsigned char *i imBuffer[++index_] = vpRGBa::alpha_default; } } - } else { + } + else { for (int i = 0; i < h; i++) { unsigned int i_ = (i_min + i) * m_rscale; unsigned int ii_ = i * pitch; @@ -420,7 +423,8 @@ void vpD3DRenderer::convertROI(const vpImage &I, unsigned char *imBuffer imBuffer[++index_] = vpRGBa::alpha_default; } } - } else { + } + else { for (int i = 0; i < h; i++) { unsigned int i_ = (i_min + i) * m_rscale; unsigned int ii_ = i * pitch; @@ -482,10 +486,10 @@ void vpD3DRenderer::setImgROI(const vpImage &im, const vpImagePoint &iP, if (pd3dDevice != nullptr) { D3DLOCKED_RECT d3dLRect; - int i_min = (std::max)((int)ceil(iP.get_i() / m_rscale), 0); - int j_min = (std::max)((int)ceil(iP.get_j() / m_rscale), 0); - int i_max = (std::min)((int)ceil((iP.get_i() + height) / m_rscale), (int)m_rheight); - int j_max = (std::min)((int)ceil((iP.get_j() + width) / m_rscale), (int)m_rwidth); + int i_min = std::max((int)ceil(iP.get_i() / m_rscale), 0); + int j_min = std::max((int)ceil(iP.get_j() / m_rscale), 0); + int i_max = std::min((int)ceil((iP.get_i() + height) / m_rscale), (int)m_rheight); + int j_max = std::min((int)ceil((iP.get_j() + width) / m_rscale), (int)m_rwidth); RECT r; r.top = (LONG)i_min; @@ -558,10 +562,10 @@ void vpD3DRenderer::setImgROI(const vpImage &im, const vpImagePoi if (pd3dDevice != nullptr) { D3DLOCKED_RECT d3dLRect; - int i_min = (std::max)((int)ceil(iP.get_i() / m_rscale), 0); - int j_min = (std::max)((int)ceil(iP.get_j() / m_rscale), 0); - int i_max = (std::min)((int)ceil((iP.get_i() + height) / m_rscale), (int)m_rheight); - int j_max = (std::min)((int)ceil((iP.get_j() + width) / m_rscale), (int)m_rwidth); + int i_min = std::max((int)ceil(iP.get_i() / m_rscale), 0); + int j_min = std::max((int)ceil(iP.get_j() / m_rscale), 0); + int i_max = std::min((int)ceil((iP.get_i() + height) / m_rscale), (int)m_rheight); + int j_max = std::min((int)ceil((iP.get_j() + width) / m_rscale), (int)m_rwidth); RECT r; r.top = (LONG)i_min; @@ -727,7 +731,8 @@ void vpD3DRenderer::drawLine(const vpImagePoint &ip1, const vpImagePoint &ip2, c if (ip2_.get_i() < ip1_.get_i()) { std::swap(ip1_, ip2_); } - } else if (ip2_.get_j() < ip1_.get_j()) { + } + else if (ip2_.get_j() < ip1_.get_j()) { std::swap(ip1_, ip2_); } @@ -746,7 +751,8 @@ void vpD3DRenderer::drawLine(const vpImagePoint &ip1, const vpImagePoint &ip2, c // Draw the line LineTo(hDCMem, vpMath::round(j / m_rscale), vpMath::round((i + deltai) / m_rscale)); } - } else { + } + else { for (unsigned int j = (unsigned int)ip1_.get_j(); j < ip2_.get_j(); j += (unsigned int)(2 * deltaj)) { double i = slope * j + orig; // Move to the starting point @@ -755,8 +761,9 @@ void vpD3DRenderer::drawLine(const vpImagePoint &ip1, const vpImagePoint &ip2, c LineTo(hDCMem, vpMath::round((j + deltaj) / m_rscale), vpMath::round((i + deltai) / m_rscale)); } } - } else { - // move to the starting point + } + else { + // move to the starting point MoveToEx(hDCMem, vpMath::round(ip1.get_u() / m_rscale), vpMath::round(ip1.get_v() / m_rscale), nullptr); // Draw the line LineTo(hDCMem, vpMath::round(ip2.get_u() / m_rscale), vpMath::round(ip2.get_v() / m_rscale)); @@ -790,7 +797,8 @@ void vpD3DRenderer::drawRect(const vpImagePoint &topLeft, unsigned int width, un drawLine(topLeft + vpImagePoint(0, width), topLeft + vpImagePoint(height, width), color, thickness); drawLine(topLeft + vpImagePoint(height, width), topLeft + vpImagePoint(height, 0), color, thickness); drawLine(topLeft + vpImagePoint(height, 0), topLeft, color, thickness); - } else { + } + else { vpImagePoint topLeftScaled = topLeft / m_rscale; unsigned int widthScaled = width / m_rscale; unsigned int heightScaled = height / m_rscale; @@ -893,12 +901,14 @@ void vpD3DRenderer::subDrawCircle(int i, int j, int x, int y, vpColor col, unsig setBufferPixel(buf, pitch, i, j - y, col, maxX, maxY); setBufferPixel(buf, pitch, i + y, j, col, maxX, maxY); setBufferPixel(buf, pitch, i - y, j, col, maxX, maxY); - } else if (x == y) { + } + else if (x == y) { setBufferPixel(buf, pitch, i + x, j + y, col, maxX, maxY); setBufferPixel(buf, pitch, i - x, j + y, col, maxX, maxY); setBufferPixel(buf, pitch, i + x, j - y, col, maxX, maxY); setBufferPixel(buf, pitch, i - x, j - y, col, maxX, maxY); - } else if (x < y) { + } + else if (x < y) { setBufferPixel(buf, pitch, i + x, j + y, col, maxX, maxY); setBufferPixel(buf, pitch, i - x, j + y, col, maxX, maxY); setBufferPixel(buf, pitch, i + x, j - y, col, maxX, maxY); @@ -934,11 +944,11 @@ void vpD3DRenderer::drawCircle(const vpImagePoint ¢er, unsigned int radius, RECT rec; int radiusScaled_ = static_cast(radiusScaled); int rleft = (vpMath::round(centerScaled.get_j() - radiusScaled_) > 0) - ? vpMath::round(centerScaled.get_j()) - radiusScaled_ - : 0; + ? vpMath::round(centerScaled.get_j()) - radiusScaled_ + : 0; int rtop = (vpMath::round(centerScaled.get_i() - radiusScaled_) > 0) - ? vpMath::round(centerScaled.get_i()) - radiusScaled_ - : 0; + ? vpMath::round(centerScaled.get_i()) - radiusScaled_ + : 0; rec.top = rtop; rec.left = rleft; @@ -978,7 +988,8 @@ void vpD3DRenderer::drawCircle(const vpImagePoint ¢er, unsigned int radius, x++; if (p < 0) { p += ((x << 1) + 1) << 1; - } else { + } + else { y--; p += (((x - y) << 1) + 1) << 1; } @@ -1085,7 +1096,8 @@ void vpD3DRenderer::drawArrow(const vpImagePoint &ip1, const vpImagePoint &ip2, if ((std::fabs(a) <= std::numeric_limits::epsilon()) && (std::fabs(b) <= std::numeric_limits::epsilon())) { // DisplayCrossLarge(i1,j1,3,col) ; - } else { + } + else { a /= lg; b /= lg; @@ -1181,6 +1193,6 @@ void vpD3DRenderer::getImage(vpImage &I) #elif !defined(VISP_BUILD_SHARED_LIBS) // Work around to avoid warning: libvisp_core.a(vpD3DRenderer.cpp.o) has no // symbols -void dummy_vpD3DRenderer(){}; +void dummy_vpD3DRenderer() { }; #endif #endif diff --git a/modules/gui/src/display/windows/vpGDIRenderer.cpp b/modules/gui/src/display/windows/vpGDIRenderer.cpp index 120161351c..eed3809185 100644 --- a/modules/gui/src/display/windows/vpGDIRenderer.cpp +++ b/modules/gui/src/display/windows/vpGDIRenderer.cpp @@ -224,7 +224,8 @@ void vpGDIRenderer::convert(const vpImage &I, HBITMAP &hBmp) imBuffer[i + 2] = I.bitmap[k].R; imBuffer[i + 3] = I.bitmap[k].A; } - } else { + } + else { for (unsigned int i = 0; i < m_rheight; i++) { unsigned int i_ = i * m_rscale; unsigned int ii_ = i * m_rwidth; @@ -255,10 +256,10 @@ void vpGDIRenderer::convert(const vpImage &I, HBITMAP &hBmp) void vpGDIRenderer::convertROI(const vpImage &I, const vpImagePoint &iP, unsigned int width, unsigned int height) { - int i_min = (std::max)((int)ceil(iP.get_i() / m_rscale), 0); - int j_min = (std::max)((int)ceil(iP.get_j() / m_rscale), 0); - int i_max = (std::min)((int)ceil((iP.get_i() + height) / m_rscale), (int)m_rheight); - int j_max = (std::min)((int)ceil((iP.get_j() + width) / m_rscale), (int)m_rwidth); + int i_min = std::max((int)ceil(iP.get_i() / m_rscale), 0); + int j_min = std::max((int)ceil(iP.get_j() / m_rscale), 0); + int i_max = std::min((int)ceil((iP.get_i() + height) / m_rscale), (int)m_rheight); + int j_max = std::min((int)ceil((iP.get_j() + width) / m_rscale), (int)m_rwidth); int h = i_max - i_min; int w = j_max - j_min; @@ -284,7 +285,8 @@ void vpGDIRenderer::convertROI(const vpImage &I, const vpImagePoint &iP, k = 0; } } - } else { + } + else { for (int i = 0; i < h; i++) { unsigned int i_ = (i_min + i) * m_rscale; unsigned int ii_ = i * w; @@ -323,7 +325,8 @@ void vpGDIRenderer::convert(const vpImage &I, HBITMAP &hBmp) imBuffer[i + 2] = I.bitmap[k]; imBuffer[i + 3] = vpRGBa::alpha_default; } - } else { + } + else { for (unsigned int i = 0; i < m_rheight; i++) { unsigned int i_ = i * m_rscale; unsigned int ii_ = i * m_rwidth; @@ -354,10 +357,10 @@ void vpGDIRenderer::convert(const vpImage &I, HBITMAP &hBmp) void vpGDIRenderer::convertROI(const vpImage &I, const vpImagePoint &iP, unsigned int width, unsigned int height) { - int i_min = (std::max)((int)ceil(iP.get_i() / m_rscale), 0); - int j_min = (std::max)((int)ceil(iP.get_j() / m_rscale), 0); - int i_max = (std::min)((int)ceil((iP.get_i() + height) / m_rscale), (int)m_rheight); - int j_max = (std::min)((int)ceil((iP.get_j() + width) / m_rscale), (int)m_rwidth); + int i_min = std::max((int)ceil(iP.get_i() / m_rscale), 0); + int j_min = std::max((int)ceil(iP.get_j() / m_rscale), 0); + int i_max = std::min((int)ceil((iP.get_i() + height) / m_rscale), (int)m_rheight); + int j_max = std::min((int)ceil((iP.get_j() + width) / m_rscale), (int)m_rwidth); int h = i_max - i_min; int w = j_max - j_min; @@ -378,7 +381,8 @@ void vpGDIRenderer::convertROI(const vpImage &I, const vpImagePoi imBuffer[++index_] = vpRGBa::alpha_default; } } - } else { + } + else { for (int i = 0; i < h; i++) { unsigned int i_ = (i_min + i) * m_rscale; unsigned int ii_ = i * w; @@ -420,7 +424,8 @@ bool vpGDIRenderer::updateBitmap(HBITMAP &hBmp, unsigned char *imBuffer, unsigne if ((m_bmp_width == w) && (m_bmp_height == h) && w != 0 && h != 0) { // just replace the content SetBitmapBits(hBmp, w * h * 4, imBuffer); - } else { + } + else { if (hBmp != nullptr) { // delete the old BITMAP DeleteObject(hBmp); @@ -601,7 +606,8 @@ void vpGDIRenderer::drawLine(const vpImagePoint &ip1, const vpImagePoint &ip2, c if (ip2_.get_i() < ip1_.get_i()) { std::swap(ip1_, ip2_); } - } else if (ip2_.get_j() < ip1_.get_j()) { + } + else if (ip2_.get_j() < ip1_.get_j()) { std::swap(ip1_, ip2_); } @@ -620,7 +626,8 @@ void vpGDIRenderer::drawLine(const vpImagePoint &ip1, const vpImagePoint &ip2, c // Draw the line LineTo(hDCMem, vpMath::round(j / m_rscale), vpMath::round((i + deltai) / m_rscale)); } - } else { + } + else { for (unsigned int j = (unsigned int)ip1_.get_j(); j < ip2_.get_j(); j += (unsigned int)(2 * deltaj)) { double i = slope * j + orig; // Move to the starting point @@ -629,8 +636,9 @@ void vpGDIRenderer::drawLine(const vpImagePoint &ip1, const vpImagePoint &ip2, c LineTo(hDCMem, vpMath::round((j + deltaj) / m_rscale), vpMath::round((i + deltai) / m_rscale)); } } - } else { - // move to the starting point + } + else { + // move to the starting point MoveToEx(hDCMem, vpMath::round(ip1.get_u() / m_rscale), vpMath::round(ip1.get_v() / m_rscale), nullptr); // Draw the line LineTo(hDCMem, vpMath::round(ip2.get_u() / m_rscale), vpMath::round(ip2.get_v() / m_rscale)); @@ -681,7 +689,8 @@ void vpGDIRenderer::drawRect(const vpImagePoint &topLeft, unsigned int width, un else { lBrush.lbColor = gdicolor; } - } else + } + else lBrush.lbStyle = BS_HOLLOW; HBRUSH hbrush = CreateBrushIndirect(&lBrush); @@ -893,7 +902,8 @@ void vpGDIRenderer::drawCross(const vpImagePoint &ip, unsigned int size, const v DeleteObject(hPen); DeleteDC(hDCMem); ReleaseDC(m_hWnd, hDCScreen); - } else { + } + else { setPixel(ip, color); } } @@ -940,7 +950,8 @@ void vpGDIRenderer::drawArrow(const vpImagePoint &ip1, const vpImagePoint &ip2, if ((a == 0) && (b == 0)) { // DisplayCrossLarge(i1,j1,3,col) ; - } else { + } + else { a /= lg; b /= lg; @@ -1020,5 +1031,5 @@ void vpGDIRenderer::getImage(vpImage &I) #elif !defined(VISP_BUILD_SHARED_LIBS) // Work around to avoid warning: libvisp_core.a(vpGDIRenderer.cpp.o) has no // symbols -void dummy_vpGDIRenderer(){}; +void dummy_vpGDIRenderer() { }; #endif diff --git a/modules/imgproc/include/visp3/imgproc/vpCircleHoughTransform.h b/modules/imgproc/include/visp3/imgproc/vpCircleHoughTransform.h index 5d7ab2def5..e4f1426b5a 100644 --- a/modules/imgproc/include/visp3/imgproc/vpCircleHoughTransform.h +++ b/modules/imgproc/include/visp3/imgproc/vpCircleHoughTransform.h @@ -216,8 +216,8 @@ class VISP_EXPORT vpCircleHoughTransform , m_upperCannyThreshRatio(upperCannyThreshRatio) , m_centerXlimits(centerXlimits) , m_centerYlimits(centerYlimits) - , m_minRadius(std::min(minRadius, maxRadius)) - , m_maxRadius(std::max(minRadius, maxRadius)) + , m_minRadius(std::min(minRadius, maxRadius)) + , m_maxRadius(std::max(minRadius, maxRadius)) , m_dilatationKernelSize(dilatationKernelSize) , m_averagingWindowSize(averagingWindowSize) , m_centerMinThresh(centerThresh) @@ -529,8 +529,8 @@ class VISP_EXPORT vpCircleHoughTransform params.m_centerXlimits = j.value("centerXlimits", params.m_centerXlimits); params.m_centerYlimits = j.value("centerYlimits", params.m_centerYlimits); std::pair radiusLimits = j.value("radiusLimits", std::pair(params.m_minRadius, params.m_maxRadius)); - params.m_minRadius = std::min(radiusLimits.first, radiusLimits.second); - params.m_maxRadius = std::max(radiusLimits.first, radiusLimits.second); + params.m_minRadius = std::min(radiusLimits.first, radiusLimits.second); + params.m_maxRadius = std::max(radiusLimits.first, radiusLimits.second); params.m_dilatationKernelSize = j.value("dilatationKernelSize", params.m_dilatationKernelSize); diff --git a/modules/imgproc/src/vpCLAHE.cpp b/modules/imgproc/src/vpCLAHE.cpp index 2338ebeb4f..3d97d5a848 100644 --- a/modules/imgproc/src/vpCLAHE.cpp +++ b/modules/imgproc/src/vpCLAHE.cpp @@ -120,10 +120,10 @@ void createHistogram(int blockRadius, int bins, int blockXCenter, int blockYCent { std::fill(hist.begin(), hist.end(), 0); - 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 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); for (int y = yMin; y < yMax; ++y) { for (int x = xMin; x < xMax; ++x) { @@ -287,8 +287,8 @@ void clahe(const vpImage &I1, vpImage &I2, int blo std::vector bl; for (int r = 0; r <= (int)rs.size(); ++r) { - int r0 = std::max(0, r - 1); - int r1 = std::min((int)rs.size() - 1, r); + int r0 = std::max(0, r - 1); + int r1 = std::min((int)rs.size() - 1, r); int dr = rs[r1] - rs[r0]; createHistogram(blockRadius, bins, cs[0], rs[r0], I1, hist); @@ -305,8 +305,8 @@ void clahe(const vpImage &I1, vpImage &I2, int blo int yMax = (r < (int)rs.size() ? rs[r1] : I1.getHeight()); for (int c = 0; c <= (int)cs.size(); ++c) { - int c0 = std::max(0, c - 1); - int c1 = std::min((int)cs.size() - 1, c); + int c0 = std::max(0, c - 1); + int c1 = std::min((int)cs.size() - 1, c); int dc = cs[c1] - cs[c0]; tl = tr; @@ -348,7 +348,7 @@ void clahe(const vpImage &I1, vpImage &I2, int blo } float t = (r0 == r1) ? t0 : wy * t0 + (1.0f - wy) * t1; - I2[y][x] = std::max(0, std::min(255, fastRound(t * 255.0f))); + I2[y][x] = std::max(0, std::min(255, fastRound(t * 255.0f))); } } } @@ -360,11 +360,11 @@ 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((int)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 yMin = std::max(0, y - (int)blockRadius); + int yMax = std::min((int)I1.getHeight(), y + blockRadius + 1); int h = yMax - yMin; #if 0 @@ -408,7 +408,7 @@ void clahe(const vpImage &I1, vpImage &I2, int blo #endif for (int x = 0; x < (int)I1.getWidth(); x++) { - int xMin = std::max(0, x - (int)blockRadius); + int xMin = std::max(0, x - (int)blockRadius); int xMax = x + blockRadius + 1; if (xMin > 0) { @@ -428,7 +428,7 @@ void clahe(const vpImage &I1, vpImage &I2, int blo } int v = fastRound(I1[y][x] / 255.0f * bins); - int w = std::min((int)I1.getWidth(), xMax) - xMin; + int w = std::min((int)I1.getWidth(), xMax) - xMin; int n = h * w; int limit = (int)(slope * n / bins + 0.5f); I2[y][x] = fastRound(transferValue(v, hist, clippedHist, limit) * 255.0f); diff --git a/modules/imgproc/src/vpCircleHoughTransform.cpp b/modules/imgproc/src/vpCircleHoughTransform.cpp index 3ca133ab16..9a457f74a5 100644 --- a/modules/imgproc/src/vpCircleHoughTransform.cpp +++ b/modules/imgproc/src/vpCircleHoughTransform.cpp @@ -195,7 +195,7 @@ vpCircleHoughTransform::detect(const vpImage &I, const int &nbCir limitMin = nbDetections; } else { - limitMin = std::min(nbDetections, (size_t)nbCircles); + limitMin = std::min(nbDetections, (size_t)nbCircles); } std::vector bestCircles; @@ -378,9 +378,9 @@ vpCircleHoughTransform::computeCenterCandidates() // The miminum horizontal position of the center is at worst -maxRadius outside the image // The maxinum horizontal position of the center is at worst +maxRadiusoutside the image // The width of the accumulator is the difference between the max and the min - int minimumXposition = std::max(m_algoParams.m_centerXlimits.first, -1 * static_cast(m_algoParams.m_maxRadius)); - int maximumXposition = std::min(m_algoParams.m_centerXlimits.second, static_cast(m_algoParams.m_maxRadius + nbCols)); - minimumXposition = std::min(minimumXposition, maximumXposition - 1); + int minimumXposition = std::max(m_algoParams.m_centerXlimits.first, -1 * static_cast(m_algoParams.m_maxRadius)); + int maximumXposition = std::min(m_algoParams.m_centerXlimits.second, static_cast(m_algoParams.m_maxRadius + nbCols)); + minimumXposition = std::min(minimumXposition, maximumXposition - 1); float minimumXpositionFloat = static_cast(minimumXposition); float maximumXpositionFloat = static_cast(maximumXposition); int offsetX = minimumXposition; @@ -393,9 +393,9 @@ vpCircleHoughTransform::computeCenterCandidates() // The miminum vertical position of the center is at worst -maxRadius outside the image // The maxinum vertical position of the center is at worst +maxRadiusoutside the image // The height of the accumulator is the difference between the max and the min - int minimumYposition = std::max(m_algoParams.m_centerYlimits.first, -1 * static_cast(m_algoParams.m_maxRadius)); - int maximumYposition = std::min(m_algoParams.m_centerYlimits.second, static_cast(m_algoParams.m_maxRadius + nbCols)); - minimumYposition = std::min(minimumYposition, maximumYposition - 1); + int minimumYposition = std::max(m_algoParams.m_centerYlimits.first, -1 * static_cast(m_algoParams.m_maxRadius)); + int maximumYposition = std::min(m_algoParams.m_centerYlimits.second, static_cast(m_algoParams.m_maxRadius + nbCols)); + minimumYposition = std::min(minimumYposition, maximumYposition - 1); float minimumYpositionFloat = static_cast(minimumYposition); float maximumYpositionFloat = static_cast(maximumYposition); int offsetY = minimumYposition; @@ -439,33 +439,33 @@ vpCircleHoughTransform::computeCenterCandidates() float max_minus_r = maximumYpositionFloat - static_cast(r); if (sx > 0) { float rmin = min_minus_c / sx; - rstart = std::max(rmin, m_algoParams.m_minRadius); + rstart = std::max(rmin, m_algoParams.m_minRadius); float rmax = max_minus_c / sx; - rstop = std::min(rmax, m_algoParams.m_maxRadius); + rstop = std::min(rmax, m_algoParams.m_maxRadius); } else if (sx < 0) { float rmin = max_minus_c / sx; - rstart = std::max(rmin, m_algoParams.m_minRadius); + rstart = std::max(rmin, m_algoParams.m_minRadius); float rmax = min_minus_c / sx; - rstop = std::min(rmax, m_algoParams.m_maxRadius); + rstop = std::min(rmax, m_algoParams.m_maxRadius); } if (sy > 0) { float rmin = min_minus_r / sy; - rstart = std::max(rmin, rstart); + rstart = std::max(rmin, rstart); float rmax = max_minus_r / sy; - rstop = std::min(rmax, rstop); + rstop = std::min(rmax, rstop); } else if (sy < 0) { float rmin = max_minus_r / sy; - rstart = std::max(rmin, rstart); + rstart = std::max(rmin, rstart); float rmax = min_minus_r / sy; - rstop = std::min(rmax, rstop); + rstop = std::min(rmax, rstop); } float deltar_x = 1.f / std::abs(sx); float deltar_y = 1.f / std::abs(sy); - float deltar = std::min(deltar_x, deltar_y); + float deltar = std::min(deltar_x, deltar_y); for (float rad = rstart; rad <= rstop && !hasToStopLoop; rad += deltar) { float x1 = static_cast(c) + rad * sx; @@ -552,7 +552,7 @@ vpCircleHoughTransform::computeCenterCandidates() // Use dilatation with large kernel in order to determine the // accumulator maxima vpImage centerCandidatesMaxima = centersAccum; - int dilatationKernelSize = std::max(m_algoParams.m_dilatationKernelSize, 3); // Ensure at least a 3x3 dilatation operation is performed + int dilatationKernelSize = std::max(m_algoParams.m_dilatationKernelSize, 3); // Ensure at least a 3x3 dilatation operation is performed vpImageMorphology::dilatation(centerCandidatesMaxima, dilatationKernelSize); // Look for the image points that correspond to the accumulator maxima @@ -572,17 +572,17 @@ vpCircleHoughTransform::computeCenterCandidates() if (left < 0) { left = x; } - nbVotes = std::max(nbVotes, static_cast(centersAccum[y][x])); + nbVotes = std::max(nbVotes, static_cast(centersAccum[y][x])); } else if (left >= 0) { int cx = static_cast((left + x - 1) * 0.5f); float sumVotes = 0.; float x_avg = 0., y_avg = 0.; int averagingWindowHalfSize = m_algoParams.m_averagingWindowSize / 2; - int startingRow = std::max(0, y - averagingWindowHalfSize); - int startingCol = std::max(0, cx - averagingWindowHalfSize); - int endRow = std::min(accumulatorHeight, y + averagingWindowHalfSize + 1); - int endCol = std::min(accumulatorWidth, cx + averagingWindowHalfSize + 1); + int startingRow = std::max(0, y - averagingWindowHalfSize); + int startingCol = std::max(0, cx - averagingWindowHalfSize); + int endRow = std::min(accumulatorHeight, y + averagingWindowHalfSize + 1); + int endCol = std::min(accumulatorWidth, cx + averagingWindowHalfSize + 1); for (int r = startingRow; r < endRow; r++) { for (int c = startingCol; c < endCol; c++) { sumVotes += centersAccum[r][c]; @@ -666,16 +666,15 @@ vpCircleHoughTransform::computeCenterCandidates() } 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; + const std::pair, float> &position_vote_b) { + return position_vote_a.second > position_vote_b.second; }; std::sort(merged_peaks_position_votes.begin(), merged_peaks_position_votes.end(), sortingCenters); nbPeaks = merged_peaks_position_votes.size(); int nbPeaksToKeep = (m_algoParams.m_expectedNbCenters > 0 ? m_algoParams.m_expectedNbCenters : nbPeaks); - nbPeaksToKeep = std::min(nbPeaksToKeep, (int)nbPeaks); + nbPeaksToKeep = std::min(nbPeaksToKeep, (int)nbPeaks); for (int i = 0; i < nbPeaksToKeep; i++) { m_centerCandidatesList.push_back(merged_peaks_position_votes[i].first); m_centerVotes.push_back(merged_peaks_position_votes[i].second); @@ -688,7 +687,7 @@ vpCircleHoughTransform::computeCircleCandidates() { size_t nbCenterCandidates = m_centerCandidatesList.size(); int nbBins = static_cast((m_algoParams.m_maxRadius - m_algoParams.m_minRadius + 1)/ m_algoParams.m_mergingRadiusDiffThresh); - nbBins = std::max((int)1, nbBins); // Avoid having 0 bins, which causes segfault + nbBins = std::max((int)1, nbBins); // Avoid having 0 bins, which causes segfault std::vector radiusAccumList; /*!< Radius accumulator for each center candidates.*/ std::vector radiusActualValueList; /*!< Vector that contains the actual distance between the edge points and the center candidates.*/ @@ -720,7 +719,7 @@ vpCircleHoughTransform::computeCircleCandidates() // Look for the Radius Candidate Bin RCB_k to which d_ij is "the closest" will have an additional vote float r = static_cast(std::sqrt(r2)); int r_bin = static_cast(std::floor((r - m_algoParams.m_minRadius) / m_algoParams.m_mergingRadiusDiffThresh)); - r_bin = std::min(r_bin, nbBins - 1); + r_bin = std::min(r_bin, nbBins - 1); if ((r < (m_algoParams.m_minRadius + m_algoParams.m_mergingRadiusDiffThresh * 0.5f)) || (r >(m_algoParams.m_minRadius + m_algoParams.m_mergingRadiusDiffThresh * (nbBins - 1 + 0.5f)))) { // If the radius is at the very beginning of the allowed radii or at the very end, we do not span the vote @@ -820,7 +819,7 @@ vpCircleHoughTransform::computeCircleProbability(const vpImageCircle &circle, co proba = 0.f; } else { - proba = std::min(visibleArc / theoreticalLenght, 1.f); + proba = std::min(visibleArc / theoreticalLenght, 1.f); } return proba; } diff --git a/modules/imgproc/src/vpMorph.cpp b/modules/imgproc/src/vpMorph.cpp index 8faa2fa136..bfb8333899 100644 --- a/modules/imgproc/src/vpMorph.cpp +++ b/modules/imgproc/src/vpMorph.cpp @@ -141,7 +141,7 @@ void reconstruct(const vpImage &marker, const vpImage(h_kp1[i][j], mask[i][j]); } } diff --git a/modules/imgproc/src/vpRetinex.cpp b/modules/imgproc/src/vpRetinex.cpp index dea7f9a4da..c05b4c0555 100644 --- a/modules/imgproc/src/vpRetinex.cpp +++ b/modules/imgproc/src/vpRetinex.cpp @@ -154,7 +154,7 @@ 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 = (int)(std::min(I.getWidth(), I.getHeight()) / 2.0); kernelSize = (kernelSize - kernelSize % 2) + 1; } diff --git a/modules/imgproc/src/vpThreshold.cpp b/modules/imgproc/src/vpThreshold.cpp index c1ab6ea9f9..fe3fc93ab7 100644 --- a/modules/imgproc/src/vpThreshold.cpp +++ b/modules/imgproc/src/vpThreshold.cpp @@ -89,7 +89,7 @@ int computeThresholdHuang(const vpHistogram &hist) S[0] = (float)hist[0]; W[0] = 0.0f; - for (size_t i = std::max((size_t)1, first); i <= last; i++) { + 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]; } diff --git a/modules/robot/src/haptic-device/qbdevice/vpQbDevice.cpp b/modules/robot/src/haptic-device/qbdevice/vpQbDevice.cpp index c806b68a9f..20792b37d3 100644 --- a/modules/robot/src/haptic-device/qbdevice/vpQbDevice.cpp +++ b/modules/robot/src/haptic-device/qbdevice/vpQbDevice.cpp @@ -136,7 +136,7 @@ int vpQbDevice::Impl::activate(const int &id, const bool &command, const int &ma failures = isActive(id, max_repeats, status); if (status != command) { m_device_api->activate(&m_file_descriptors.at(m_connected_devices.at(id)), id, command); - failures = std::max(failures, isActive(id, max_repeats, status)); + failures = std::max(failures, isActive(id, max_repeats, status)); if (status != command) { std::cout << "Device [" << id << "] fails on " << command_prefix << "activation." << std::endl; ; diff --git a/modules/robot/src/real-robot/bebop2/vpRobotBebop2.cpp b/modules/robot/src/real-robot/bebop2/vpRobotBebop2.cpp index 431172b633..feb38b1155 100644 --- a/modules/robot/src/real-robot/bebop2/vpRobotBebop2.cpp +++ b/modules/robot/src/real-robot/bebop2/vpRobotBebop2.cpp @@ -184,7 +184,8 @@ vpRobotBebop2::vpRobotBebop2(bool verbose, bool setDefaultSettings, std::string "Failed to connect to bebop2 with ip %s and port %d. Make sure that the ip address is correct " "and that your computer is connected to the drone Wifi spot before starting", ipAddress.c_str(), discoveryPort)); - } else { + } + else { m_running = true; #ifdef VISP_HAVE_FFMPEG @@ -230,7 +231,8 @@ void vpRobotBebop2::doFlatTrim() while (!m_flatTrimFinished) { vpTime::sleepMs(1); } - } else { + } + else { ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't do a flat trim : drone isn't landed."); } } @@ -328,7 +330,8 @@ void vpRobotBebop2::setCameraOrientation(double tilt, double pan, bool blocking) } } - } else { + } + else { ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set camera orientation : drone isn't running."); } } @@ -357,7 +360,8 @@ void vpRobotBebop2::setCameraTilt(double tilt, bool blocking) } } - } else { + } + else { ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set camera tilt value : drone isn't running."); } } @@ -386,7 +390,8 @@ void vpRobotBebop2::setCameraPan(double pan, bool blocking) } } - } else { + } + else { ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set camera pan value : drone isn't running."); } } @@ -399,7 +404,8 @@ bool vpRobotBebop2::isRunning() { if (m_deviceController == nullptr) { return false; - } else { + } + else { return m_running; } } @@ -463,7 +469,8 @@ void vpRobotBebop2::takeOff(bool blocking) } } - } else { + } + else { ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't take off : drone isn't landed."); } } @@ -495,14 +502,15 @@ void vpRobotBebop2::setVerticalSpeed(int value) { if (isRunning() && m_deviceController != nullptr && (isFlying() || isHovering())) { m_errorController = - m_deviceController->aRDrone3->setPilotingPCMDGaz(m_deviceController->aRDrone3, static_cast(value)); + m_deviceController->aRDrone3->setPilotingPCMDGaz(m_deviceController->aRDrone3, static_cast(value)); if (m_errorController != ARCONTROLLER_OK) { ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error when sending move command : %s", ARCONTROLLER_Error_ToString(m_errorController)); } - } else { + } + else { ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set vertical speed : drone isn't flying or hovering."); } } @@ -522,14 +530,15 @@ void vpRobotBebop2::setYawSpeed(int value) if (isRunning() && m_deviceController != nullptr && (isFlying() || isHovering())) { m_errorController = - m_deviceController->aRDrone3->setPilotingPCMDYaw(m_deviceController->aRDrone3, static_cast(value)); + m_deviceController->aRDrone3->setPilotingPCMDYaw(m_deviceController->aRDrone3, static_cast(value)); if (m_errorController != ARCONTROLLER_OK) { ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error when sending move command : %s", ARCONTROLLER_Error_ToString(m_errorController)); } - } else { + } + else { ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set yaw speed : drone isn't flying or hovering."); } } @@ -549,7 +558,7 @@ void vpRobotBebop2::setPitch(int value) if (isRunning() && m_deviceController != nullptr && (isFlying() || isHovering())) { m_errorController = - m_deviceController->aRDrone3->setPilotingPCMDPitch(m_deviceController->aRDrone3, static_cast(value)); + m_deviceController->aRDrone3->setPilotingPCMDPitch(m_deviceController->aRDrone3, static_cast(value)); m_errorController = m_deviceController->aRDrone3->setPilotingPCMDFlag(m_deviceController->aRDrone3, 1); if (m_errorController != ARCONTROLLER_OK) { @@ -557,7 +566,8 @@ void vpRobotBebop2::setPitch(int value) ARCONTROLLER_Error_ToString(m_errorController)); } - } else { + } + else { ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set pitch value : drone isn't flying or hovering."); } } @@ -577,7 +587,7 @@ void vpRobotBebop2::setRoll(int value) if (isRunning() && m_deviceController != nullptr && (isFlying() || isHovering())) { m_errorController = - m_deviceController->aRDrone3->setPilotingPCMDRoll(m_deviceController->aRDrone3, static_cast(value)); + m_deviceController->aRDrone3->setPilotingPCMDRoll(m_deviceController->aRDrone3, static_cast(value)); m_errorController = m_deviceController->aRDrone3->setPilotingPCMDFlag(m_deviceController->aRDrone3, 1); if (m_errorController != ARCONTROLLER_OK) { @@ -585,7 +595,8 @@ void vpRobotBebop2::setRoll(int value) ARCONTROLLER_Error_ToString(m_errorController)); } - } else { + } + else { ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set roll value : drone isn't flying or hovering."); } } @@ -632,7 +643,8 @@ void vpRobotBebop2::setPosition(float dX, float dY, float dZ, float dPsi, bool b vpTime::sleepMs(1); } } - } else { + } + else { ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't move : drone isn't flying or hovering."); } } @@ -710,7 +722,8 @@ void vpRobotBebop2::setVerbose(bool verbose) { if (verbose) { ARSAL_Print_SetMinimumLevel(ARSAL_PRINT_INFO); - } else { + } + else { ARSAL_Print_SetMinimumLevel(ARSAL_PRINT_WARNING); } } @@ -730,7 +743,8 @@ void vpRobotBebop2::resetAllSettings() vpTime::sleepMs(1); } - } else { + } + else { ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't reset drone settings : drone isn't running."); } } @@ -751,7 +765,8 @@ void vpRobotBebop2::setMaxTilt(double maxTilt) if (isRunning() && m_deviceController != nullptr) { m_deviceController->aRDrone3->sendPilotingSettingsMaxTilt(m_deviceController->aRDrone3, static_cast(maxTilt)); - } else { + } + else { ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set tilt value : drone isn't running."); } } @@ -795,11 +810,13 @@ void vpRobotBebop2::getGrayscaleImage(vpImage &I) vpImageConvert::BGRToGrey(m_bgr_picture->data[0], reinterpret_cast(I.bitmap), I.getWidth(), I.getHeight()); m_bgr_picture_mutex.unlock(); - } else { + } + else { ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Error while getting current grayscale image : image data is nullptr"); } - } else { + } + else { ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't get current image : video streaming isn't started."); } } @@ -823,11 +840,13 @@ void vpRobotBebop2::getRGBaImage(vpImage &I) vpImageConvert::BGRToRGBa(m_bgr_picture->data[0], reinterpret_cast(I.bitmap), I.getWidth(), I.getHeight()); m_bgr_picture_mutex.unlock(); - } else { + } + else { ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Error while getting current RGBa image : image data is nullptr"); } - } else { + } + else { ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't get current image : video streaming isn't started."); } } @@ -856,7 +875,7 @@ int vpRobotBebop2::getVideoWidth() { return m_videoWidth; } void vpRobotBebop2::setExposure(float expo) { if (isRunning() && m_deviceController != nullptr) { - expo = std::min(1.5f, std::max(-1.5f, expo)); + expo = std::min(1.5f, std::max(-1.5f, expo)); m_exposureSet = false; m_deviceController->aRDrone3->sendPictureSettingsExpositionSelection(m_deviceController->aRDrone3, expo); @@ -865,7 +884,8 @@ void vpRobotBebop2::setExposure(float expo) while (!m_exposureSet) { vpTime::sleepMs(1); } - } else { + } + else { ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set exposure : drone isn't running."); } } @@ -890,7 +910,7 @@ void vpRobotBebop2::setStreamingMode(int mode) if (!isStreaming() && isLanded()) { eARCOMMANDS_ARDRONE3_MEDIASTREAMING_VIDEOSTREAMMODE_MODE cmd_mode = - ARCOMMANDS_ARDRONE3_MEDIASTREAMING_VIDEOSTREAMMODE_MODE_LOW_LATENCY; + ARCOMMANDS_ARDRONE3_MEDIASTREAMING_VIDEOSTREAMMODE_MODE_LOW_LATENCY; switch (mode) { case 0: cmd_mode = ARCOMMANDS_ARDRONE3_MEDIASTREAMING_VIDEOSTREAMMODE_MODE_LOW_LATENCY; @@ -912,12 +932,14 @@ void vpRobotBebop2::setStreamingMode(int mode) vpTime::sleepMs(1); } - } else { + } + else { ARSAL_PRINT( ARSAL_PRINT_ERROR, "ERROR", "Can't set streaming mode : drone has to be landed and not streaming in order to set streaming mode."); } - } else { + } + else { ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set streaming mode : drone isn't running."); } } @@ -965,12 +987,14 @@ void vpRobotBebop2::setVideoResolution(int mode) vpTime::sleepMs(1); } - } else { + } + else { ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set video resolution : drone has to be landed and not streaming in order to set streaming " "parameters."); } - } else { + } + else { ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set video resolution : drone isn't running."); } } @@ -991,7 +1015,7 @@ void vpRobotBebop2::setVideoStabilisationMode(int mode) if (isRunning() && m_deviceController != nullptr) { eARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE cmd_mode = - ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE_NONE; + ARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE_NONE; switch (mode) { case 0: @@ -1012,7 +1036,8 @@ void vpRobotBebop2::setVideoStabilisationMode(int mode) } m_deviceController->aRDrone3->sendPictureSettingsVideoStabilizationMode(m_deviceController->aRDrone3, cmd_mode); - } else { + } + else { ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't set video stabilisation mode : drone isn't running."); } } @@ -1046,11 +1071,13 @@ void vpRobotBebop2::startStreaming() decoder is doesn't have time to synchronize with the stream */ vpTime::sleepMs(4000); - } else { + } + else { ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error :%s", ARCONTROLLER_Error_ToString(m_errorController)); } - } else { + } + else { ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't start streaming : drone isn't running."); } } @@ -1077,11 +1104,13 @@ void vpRobotBebop2::stopStreaming() vpTime::sleepMs(500); // We wait for the streaming to actually stops or else it causes segmentation fault. stopVideoDecoding(); - } else { + } + else { ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "- error :%s", ARCONTROLLER_Error_ToString(m_errorController)); } - } else { + } + else { ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Can't stop streaming : streaming already stopped."); } } @@ -1136,7 +1165,7 @@ eARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE vpRobotBebop2::getFl { if (m_deviceController != nullptr) { eARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE flyingState = - ARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE_MAX; + ARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE_MAX; eARCONTROLLER_ERROR error; ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary = ARCONTROLLER_ARDrone3_GetCommandElements( @@ -1161,7 +1190,8 @@ eARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE vpRobotBebop2::getFl } } return flyingState; - } else { + } + else { ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Error when checking flying state : drone isn't connected."); return ARCOMMANDS_ARDRONE3_PILOTINGSTATE_FLYINGSTATECHANGED_STATE_MAX; } @@ -1175,7 +1205,7 @@ eARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED vpRobotBebop { if (m_deviceController != nullptr) { eARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED streamingState = - ARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED_MAX; + ARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED_MAX; eARCONTROLLER_ERROR error; ARCONTROLLER_DICTIONARY_ELEMENT_t *elementDictionary = ARCONTROLLER_ARDrone3_GetCommandElements( @@ -1196,12 +1226,13 @@ eARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED vpRobotBebop if (arg != nullptr) { // Enums are stored as I32 streamingState = - static_cast(arg->value.I32); + static_cast(arg->value.I32); } } } return streamingState; - } else { + } + else { ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Error when checking streaming state : drone isn't connected."); return ARCOMMANDS_ARDRONE3_MEDIASTREAMINGSTATE_VIDEOENABLECHANGED_ENABLED_MAX; } @@ -1220,7 +1251,7 @@ ARDISCOVERY_Device_t *vpRobotBebop2::discoverDrone() ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, " - Starting drone Wifi discovery ..."); const char *charIpAddress = m_ipAddress.c_str(); errorDiscovery = - ARDISCOVERY_Device_InitWifi(device, ARDISCOVERY_PRODUCT_BEBOP_2, "bebop2", charIpAddress, m_discoveryPort); + ARDISCOVERY_Device_InitWifi(device, ARDISCOVERY_PRODUCT_BEBOP_2, "bebop2", charIpAddress, m_discoveryPort); if (errorDiscovery != ARDISCOVERY_OK) { ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Discovery error :%s", ARDISCOVERY_Error_ToString(errorDiscovery)); @@ -1412,7 +1443,8 @@ void vpRobotBebop2::startVideoDecoding() if (!m_videoDecodingStarted) { initCodec(); m_videoDecodingStarted = true; - } else { + } + else { ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Video decoding is already started."); } } @@ -1426,7 +1458,8 @@ void vpRobotBebop2::stopVideoDecoding() { if (m_videoDecodingStarted) { cleanUpCodec(); - } else { + } + else { ARSAL_PRINT(ARSAL_PRINT_ERROR, "ERROR", "Video decoding is already stopped."); } } @@ -1457,10 +1490,12 @@ void vpRobotBebop2::computeFrame(ARCONTROLLER_Frame_t *frame) if (ret == 0 || ret == AVERROR(EAGAIN)) { ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "H264 codec parameters updated."); - } else { + } + else { ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Unexpected error while updating H264 parameters."); } - } else { + } + else { ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Unexpected error while sending H264 parameters."); } m_update_codec_params = false; @@ -1481,7 +1516,8 @@ void vpRobotBebop2::computeFrame(ARCONTROLLER_Frame_t *frame) delete[] errbuff; ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Error sending a packet for decoding : %d, %s", ret, err.c_str()); - } else { + } + else { ret = avcodec_receive_frame(m_codecContext, m_picture); @@ -1489,9 +1525,11 @@ void vpRobotBebop2::computeFrame(ARCONTROLLER_Frame_t *frame) if (ret == AVERROR(EAGAIN)) { ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "AVERROR(EAGAIN)"); // Not an error, need to send data again - } else if (ret == AVERROR_EOF) { + } + else if (ret == AVERROR_EOF) { ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "AVERROR_EOF"); // End of file reached, should not happen with drone footage - } else { + } + else { char *errbuff = new char[AV_ERROR_MAX_STRING_SIZE]; av_strerror(ret, errbuff, AV_ERROR_MAX_STRING_SIZE); @@ -1499,7 +1537,8 @@ void vpRobotBebop2::computeFrame(ARCONTROLLER_Frame_t *frame) delete[] errbuff; ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Error receiving a decoded frame : %d, %s", ret, err.c_str()); } - } else { + } + else { m_bgr_picture_mutex.lock(); av_frame_unref(m_bgr_picture); av_image_fill_arrays(m_bgr_picture->data, m_bgr_picture->linesize, m_buffer, AV_PIX_FMT_BGR24, @@ -1552,7 +1591,8 @@ void vpRobotBebop2::cleanUp() ARSAL_Sem_Destroy(&(m_stateSem)); ARSAL_PRINT(ARSAL_PRINT_INFO, TAG, "- Cleanup done."); - } else { + } + else { ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Error while cleaning up memory."); } @@ -1627,8 +1667,9 @@ eARCONTROLLER_ERROR vpRobotBebop2::decoderConfigCallback(ARCONTROLLER_Stream_Cod drone->m_codec_params_data.resize(sps_buffer_size + pps_buffer_size); std::copy(sps_buffer_ptr, sps_buffer_ptr + sps_buffer_size, drone->m_codec_params_data.begin()); std::copy(pps_buffer_ptr, pps_buffer_ptr + pps_buffer_size, drone->m_codec_params_data.begin() + sps_buffer_size); - } else { - // If data is invalid, we clear the vector + } + else { + // If data is invalid, we clear the vector drone->m_codec_params_data.clear(); } return ARCONTROLLER_OK; @@ -1652,7 +1693,8 @@ eARCONTROLLER_ERROR vpRobotBebop2::didReceiveFrameCallback(ARCONTROLLER_Frame_t drone->computeFrame(frame); } - } else { + } + else { ARSAL_PRINT(ARSAL_PRINT_WARNING, TAG, "frame is nullptr."); } @@ -1701,7 +1743,8 @@ void vpRobotBebop2::cmdBatteryStateChangedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t if (drone->m_batteryLevel <= 5) { ARSAL_PRINT(ARSAL_PRINT_WARNING, TAG, " - WARNING, very low battery level, drone will stop soon !"); - } else if (drone->m_batteryLevel <= 10) { + } + else if (drone->m_batteryLevel <= 10) { ARSAL_PRINT(ARSAL_PRINT_WARNING, TAG, " - Warning, low battery level !"); } } @@ -1839,7 +1882,7 @@ void vpRobotBebop2::cmdRelativeMoveEndedRcv(ARCONTROLLER_DICTIONARY_ELEMENT_t *e if (arg != nullptr) { eARCOMMANDS_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR error = - static_cast(arg->value.I32); + static_cast(arg->value.I32); if ((error != ARCOMMANDS_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR_OK) && (error != ARCOMMANDS_ARDRONE3_PILOTINGEVENT_MOVEBYEND_ERROR_INTERRUPTED)) { ARSAL_PRINT(ARSAL_PRINT_ERROR, TAG, "Relative move ended with error %d", error); @@ -1963,5 +2006,5 @@ void vpRobotBebop2::commandReceivedCallback(eARCONTROLLER_DICTIONARY_KEY command #elif !defined(VISP_BUILD_SHARED_LIBS) // Work around to avoid warning: libvisp_robot.a(vpRobotBebop2.cpp.o) has // no symbols -void dummy_vpRobotBebop2(){}; +void dummy_vpRobotBebop2() { }; #endif // VISP_HAVE_ARSDK diff --git a/modules/robot/src/real-robot/franka/vpJointVelTrajGenerator_impl.cpp b/modules/robot/src/real-robot/franka/vpJointVelTrajGenerator_impl.cpp index 82d6b34b0a..e357b4d1ba 100644 --- a/modules/robot/src/real-robot/franka/vpJointVelTrajGenerator_impl.cpp +++ b/modules/robot/src/real-robot/franka/vpJointVelTrajGenerator_impl.cpp @@ -100,8 +100,8 @@ void vpJointVelTrajGenerator::control_thread(franka::Robot *robot, std::atomic_b for (size_t i = 0; i < 7; i++) { gnuplot << "plot 'dq-mes.log' u " << i + 1 << " title \"dq-mes" << i + 1 << "\", 'dq-des.log' u " << i + 1 - << " title \"dq-des" << i + 1 << "\", 'dq-cmd.log' u " << i + 1 << " title \"dq-cmd" << i + 1 << "\"" - << std::endl; + << " title \"dq-des" << i + 1 << "\", 'dq-cmd.log' u " << i + 1 << " title \"dq-cmd" << i + 1 << "\"" + << std::endl; gnuplot << "\npause -1\n" << std::endl; } @@ -109,8 +109,8 @@ void vpJointVelTrajGenerator::control_thread(franka::Robot *robot, std::atomic_b } auto joint_velocity_callback = - [=, &log_time, &log_q_mes, &log_dq_mes, &log_dq_des, &log_dq_cmd, &time, &q_prev, &dq_des, &stop, &robot_state, - &mutex](const franka::RobotState &state, franka::Duration period) -> franka::JointVelocities { + [=, &log_time, &log_q_mes, &log_dq_mes, &log_dq_des, &log_dq_cmd, &time, &q_prev, &dq_des, &stop, &robot_state, + &mutex](const franka::RobotState &state, franka::Duration period) -> franka::JointVelocities { time += period.toSec(); static vpJointVelTrajGenerator joint_vel_traj_generator; @@ -147,18 +147,18 @@ void vpJointVelTrajGenerator::control_thread(franka::Robot *robot, std::atomic_b if (!log_folder.empty()) { log_time << time << std::endl; log_q_mes << std::fixed << std::setprecision(8) << state.q_d[0] << " " << state.q_d[1] << " " << state.q_d[2] - << " " << state.q_d[3] << " " << state.q_d[4] << " " << state.q_d[5] << " " << state.q_d[6] - << std::endl; + << " " << state.q_d[3] << " " << state.q_d[4] << " " << state.q_d[5] << " " << state.q_d[6] + << std::endl; log_dq_mes << std::fixed << std::setprecision(8) << state.dq_d[0] << " " << state.dq_d[1] << " " << state.dq_d[2] - << " " << state.dq_d[3] << " " << state.dq_d[4] << " " << state.dq_d[5] << " " << state.dq_d[6] - << std::endl; + << " " << state.dq_d[3] << " " << state.dq_d[4] << " " << state.dq_d[5] << " " << state.dq_d[6] + << std::endl; log_dq_cmd << std::fixed << std::setprecision(8) << dq_cmd[0] << " " << dq_cmd[1] << " " << dq_cmd[2] << " " - << dq_cmd[3] << " " << dq_cmd[4] << " " << dq_cmd[5] << " " << dq_cmd[6] << std::endl; + << dq_cmd[3] << " " << dq_cmd[4] << " " << dq_cmd[5] << " " << dq_cmd[6] << std::endl; log_dq_des << std::fixed << std::setprecision(8) << dq_des_[0] << " " << dq_des_[1] << " " << dq_des_[2] << " " - << dq_des_[3] << " " << dq_des_[4] << " " << dq_des_[5] << " " << dq_des_[6] << std::endl; + << dq_des_[3] << " " << dq_des_[4] << " " << dq_des_[5] << " " << dq_des_[6] << std::endl; } - franka::JointVelocities velocities = {dq_cmd[0], dq_cmd[1], dq_cmd[2], dq_cmd[3], dq_cmd[4], dq_cmd[5], dq_cmd[6]}; + franka::JointVelocities velocities = { dq_cmd[0], dq_cmd[1], dq_cmd[2], dq_cmd[3], dq_cmd[4], dq_cmd[5], dq_cmd[6] }; if (stop) { unsigned int nb_joint_stop = 0; @@ -197,119 +197,122 @@ void vpJointVelTrajGenerator::control_thread(franka::Robot *robot, std::atomic_b // With libfranka 0.5.0 franka::control() enables limit_rate by default return velocities; #endif - }; + }; auto cartesian_velocity_callback = [=, &log_time, &log_q_mes, &log_dq_mes, &log_dq_des, &log_dq_cmd, &log_v_des, - &time, &model, &q_prev, &v_cart_des, &stop, &robot_state, - &mutex](const franka::RobotState &state, - franka::Duration period) -> franka::JointVelocities { - time += period.toSec(); + &time, &model, &q_prev, &v_cart_des, &stop, &robot_state, + &mutex](const franka::RobotState &state, + franka::Duration period) -> franka::JointVelocities { + time += period.toSec(); - static vpJointVelTrajGenerator joint_vel_traj_generator; + static vpJointVelTrajGenerator joint_vel_traj_generator; - if (time == 0.0) { - if (!log_folder.empty()) { - log_time.open(log_folder + "/time.log"); - log_q_mes.open(log_folder + "/q-mes.log"); - log_dq_mes.open(log_folder + "/dq-mes.log"); - log_dq_des.open(log_folder + "/dq-des.log"); - log_dq_cmd.open(log_folder + "/dq-cmd.log"); - log_v_des.open(log_folder + "/v-des.log"); - } - q_prev = state.q_d; - joint_vel_traj_generator.init(state.q_d, q_min, q_max, dq_max, ddq_max, delta_t); + if (time == 0.0) { + if (!log_folder.empty()) { + log_time.open(log_folder + "/time.log"); + log_q_mes.open(log_folder + "/q-mes.log"); + log_dq_mes.open(log_folder + "/dq-mes.log"); + log_dq_des.open(log_folder + "/dq-des.log"); + log_dq_cmd.open(log_folder + "/dq-cmd.log"); + log_v_des.open(log_folder + "/v-des.log"); } + q_prev = state.q_d; + joint_vel_traj_generator.init(state.q_d, q_min, q_max, dq_max, ddq_max, delta_t); + } - { - std::lock_guard lock(mutex); - robot_state = state; - } + { + std::lock_guard lock(mutex); + robot_state = state; + } - // Get robot Jacobian - if (frame == vpRobot::END_EFFECTOR_FRAME || frame == vpRobot::TOOL_FRAME) { - std::array jacobian = model.bodyJacobian(franka::Frame::kEndEffector, state); - // Convert row-major to col-major - for (size_t i = 0; i < 6; i++) { // TODO make a function - for (size_t j = 0; j < 7; j++) { - eJe[i][j] = jacobian[j * 6 + i]; - } + // Get robot Jacobian + if (frame == vpRobot::END_EFFECTOR_FRAME || frame == vpRobot::TOOL_FRAME) { + std::array jacobian = model.bodyJacobian(franka::Frame::kEndEffector, state); + // Convert row-major to col-major + for (size_t i = 0; i < 6; i++) { // TODO make a function + for (size_t j = 0; j < 7; j++) { + eJe[i][j] = jacobian[j * 6 + i]; } - } else if (frame == vpRobot::REFERENCE_FRAME) { - std::array jacobian = model.zeroJacobian(franka::Frame::kEndEffector, state); - // Convert row-major to col-major - for (size_t i = 0; i < 6; i++) { // TODO make a function - for (size_t j = 0; j < 7; j++) { - fJe[i][j] = jacobian[j * 6 + i]; - } + } + } + else if (frame == vpRobot::REFERENCE_FRAME) { + std::array jacobian = model.zeroJacobian(franka::Frame::kEndEffector, state); + // Convert row-major to col-major + for (size_t i = 0; i < 6; i++) { // TODO make a function + for (size_t j = 0; j < 7; j++) { + fJe[i][j] = jacobian[j * 6 + i]; } } + } - // Compute joint velocity - vpColVector q_dot; - if (frame == vpRobot::END_EFFECTOR_FRAME) { - q_dot = eJe.pseudoInverse() * v_cart_des; // TODO introduce try catch - } else if (frame == vpRobot::TOOL_FRAME) { - q_dot = (cVe * eJe).pseudoInverse() * v_cart_des; // TODO introduce try catch - } else if (frame == vpRobot::REFERENCE_FRAME) { - q_dot = (cVe * fJe).pseudoInverse() * v_cart_des; // TODO introduce try catch - } + // Compute joint velocity + vpColVector q_dot; + if (frame == vpRobot::END_EFFECTOR_FRAME) { + q_dot = eJe.pseudoInverse() * v_cart_des; // TODO introduce try catch + } + else if (frame == vpRobot::TOOL_FRAME) { + q_dot = (cVe * eJe).pseudoInverse() * v_cart_des; // TODO introduce try catch + } + else if (frame == vpRobot::REFERENCE_FRAME) { + q_dot = (cVe * fJe).pseudoInverse() * v_cart_des; // TODO introduce try catch + } - std::array dq_des_eigen; - for (size_t i = 0; i < 7; i++) // TODO create a function to convert - dq_des_eigen[i] = q_dot[i]; + std::array dq_des_eigen; + for (size_t i = 0; i < 7; i++) // TODO create a function to convert + dq_des_eigen[i] = q_dot[i]; - std::array q_cmd; - std::array dq_cmd; + std::array q_cmd; + std::array dq_cmd; - auto dq_des_ = dq_des_eigen; - if (stop) { // Stop asked - for (auto &dq_ : dq_des_) { - dq_ = 0.0; - } + auto dq_des_ = dq_des_eigen; + if (stop) { // Stop asked + for (auto &dq_ : dq_des_) { + dq_ = 0.0; } + } - joint_vel_traj_generator.applyVel(dq_des_, q_cmd, dq_cmd); + joint_vel_traj_generator.applyVel(dq_des_, q_cmd, dq_cmd); - if (!log_folder.empty()) { - log_time << time << std::endl; - log_q_mes << std::fixed << std::setprecision(8) << state.q_d[0] << " " << state.q_d[1] << " " << state.q_d[2] - << " " << state.q_d[3] << " " << state.q_d[4] << " " << state.q_d[5] << " " << state.q_d[6] - << std::endl; - log_dq_mes << std::fixed << std::setprecision(8) << state.dq_d[0] << " " << state.dq_d[1] << " " << state.dq_d[2] - << " " << state.dq_d[3] << " " << state.dq_d[4] << " " << state.dq_d[5] << " " << state.dq_d[6] - << std::endl; - log_dq_cmd << std::fixed << std::setprecision(8) << dq_cmd[0] << " " << dq_cmd[1] << " " << dq_cmd[2] << " " - << dq_cmd[3] << " " << dq_cmd[4] << " " << dq_cmd[5] << " " << dq_cmd[6] << std::endl; - log_dq_des << std::fixed << std::setprecision(8) << dq_des_[0] << " " << dq_des_[1] << " " << dq_des_[2] << " " - << dq_des_[3] << " " << dq_des_[4] << " " << dq_des_[5] << " " << dq_des_[6] << std::endl; - log_v_des << std::fixed << std::setprecision(8) << v_cart_des[0] << " " << v_cart_des[1] << " " << v_cart_des[2] - << " " << v_cart_des[3] << " " << v_cart_des[4] << " " << v_cart_des[5] << std::endl; - } + if (!log_folder.empty()) { + log_time << time << std::endl; + log_q_mes << std::fixed << std::setprecision(8) << state.q_d[0] << " " << state.q_d[1] << " " << state.q_d[2] + << " " << state.q_d[3] << " " << state.q_d[4] << " " << state.q_d[5] << " " << state.q_d[6] + << std::endl; + log_dq_mes << std::fixed << std::setprecision(8) << state.dq_d[0] << " " << state.dq_d[1] << " " << state.dq_d[2] + << " " << state.dq_d[3] << " " << state.dq_d[4] << " " << state.dq_d[5] << " " << state.dq_d[6] + << std::endl; + log_dq_cmd << std::fixed << std::setprecision(8) << dq_cmd[0] << " " << dq_cmd[1] << " " << dq_cmd[2] << " " + << dq_cmd[3] << " " << dq_cmd[4] << " " << dq_cmd[5] << " " << dq_cmd[6] << std::endl; + log_dq_des << std::fixed << std::setprecision(8) << dq_des_[0] << " " << dq_des_[1] << " " << dq_des_[2] << " " + << dq_des_[3] << " " << dq_des_[4] << " " << dq_des_[5] << " " << dq_des_[6] << std::endl; + log_v_des << std::fixed << std::setprecision(8) << v_cart_des[0] << " " << v_cart_des[1] << " " << v_cart_des[2] + << " " << v_cart_des[3] << " " << v_cart_des[4] << " " << v_cart_des[5] << std::endl; + } - franka::JointVelocities velocities = {dq_cmd[0], dq_cmd[1], dq_cmd[2], dq_cmd[3], dq_cmd[4], dq_cmd[5], dq_cmd[6]}; + franka::JointVelocities velocities = { dq_cmd[0], dq_cmd[1], dq_cmd[2], dq_cmd[3], dq_cmd[4], dq_cmd[5], dq_cmd[6] }; - if (stop) { - unsigned int nb_joint_stop = 0; - const double q_eps = 1e-6; // Motion finished - for (size_t i = 0; i < 7; i++) { - if (std::abs(state.q_d[i] - q_prev[i]) < q_eps) { - nb_joint_stop++; - } + if (stop) { + unsigned int nb_joint_stop = 0; + const double q_eps = 1e-6; // Motion finished + for (size_t i = 0; i < 7; i++) { + if (std::abs(state.q_d[i] - q_prev[i]) < q_eps) { + nb_joint_stop++; } - if (nb_joint_stop == 7) { - if (!log_folder.empty()) { - log_time.close(); - log_q_mes.close(); - log_dq_mes.close(); - log_dq_des.close(); - log_dq_cmd.close(); - log_v_des.close(); - } - return franka::MotionFinished(velocities); + } + if (nb_joint_stop == 7) { + if (!log_folder.empty()) { + log_time.close(); + log_q_mes.close(); + log_dq_mes.close(); + log_dq_des.close(); + log_dq_cmd.close(); + log_v_des.close(); } + return franka::MotionFinished(velocities); } + } - q_prev = state.q_d; + q_prev = state.q_d; #if (VISP_HAVE_FRANKA_VERSION < 0x000500) // state.q_d contains the last joint velocity command received by the robot. @@ -320,12 +323,12 @@ void vpJointVelTrajGenerator::control_thread(franka::Robot *robot, std::atomic_b // by the robot will prevent from getting discontinuity errors. // Note that if the robot does not receive a command it will try to extrapolate // the desired behavior assuming a constant acceleration model - return limitRate(ddq_max, velocities.dq, state.dq_d); + return limitRate(ddq_max, velocities.dq, state.dq_d); #else // With libfranka 0.5.0 franka::control enables limit_rate by default - return velocities; + return velocities; #endif - }; + }; #if !(VISP_HAVE_FRANKA_VERSION < 0x000500) double cutoff_frequency = 10; @@ -341,7 +344,8 @@ void vpJointVelTrajGenerator::control_thread(franka::Robot *robot, std::atomic_b robot->control(joint_velocity_callback, franka::ControllerMode::kJointImpedance, true, cutoff_frequency); #endif break; - } catch (const franka::ControlException &e) { + } + catch (const franka::ControlException &e) { std::cerr << "Warning: communication error: " << e.what() << "\nRetry attempt: " << attempt << std::endl; robot->automaticErrorRecovery(); if (attempt == nbAttempts) @@ -362,7 +366,8 @@ void vpJointVelTrajGenerator::control_thread(franka::Robot *robot, std::atomic_b robot->control(cartesian_velocity_callback, franka::ControllerMode::kJointImpedance, true, cutoff_frequency); #endif break; - } catch (const franka::ControlException &e) { + } + catch (const franka::ControlException &e) { std::cerr << "Warning: communication error: " << e.what() << "\nRetry attempt: " << attempt << std::endl; robot->automaticErrorRecovery(); if (attempt == nbAttempts) @@ -395,15 +400,15 @@ void vpJointVelTrajGenerator::init(const std::array &q, const std::ar m_q_final = m_q_cmd = q; m_q_cmd_prev = m_q_cmd; - m_dq_des = {0, 0, 0, 0, 0, 0, 0}; - m_dq_des_prev = {0, 0, 0, 0, 0, 0, 0}; - m_delta_q = {0, 0, 0, 0, 0, 0, 0}; - m_delta_q_max = {0, 0, 0, 0, 0, 0, 0}; - m_sign = {0, 0, 0, 0, 0, 0, 0}; - m_dist_to_final = {0, 0, 0, 0, 0, 0, 0}; - m_dist_AD = {0, 0, 0, 0, 0, 0, 0}; - m_flagSpeed = {false, false, false, false, false, false, false}; - m_status = {FLAGSTO, FLAGSTO, FLAGSTO, FLAGSTO, FLAGSTO, FLAGSTO, FLAGSTO}; + m_dq_des = { 0, 0, 0, 0, 0, 0, 0 }; + m_dq_des_prev = { 0, 0, 0, 0, 0, 0, 0 }; + m_delta_q = { 0, 0, 0, 0, 0, 0, 0 }; + m_delta_q_max = { 0, 0, 0, 0, 0, 0, 0 }; + m_sign = { 0, 0, 0, 0, 0, 0, 0 }; + m_dist_to_final = { 0, 0, 0, 0, 0, 0, 0 }; + m_dist_AD = { 0, 0, 0, 0, 0, 0, 0 }; + m_flagSpeed = { false, false, false, false, false, false, false }; + m_status = { FLAGSTO, FLAGSTO, FLAGSTO, FLAGSTO, FLAGSTO, FLAGSTO, FLAGSTO }; for (size_t i = 0; i < m_njoints; i++) { m_delta_q_acc[i] = m_ddq_max[i] * m_delta_t * m_delta_t; @@ -428,7 +433,8 @@ void vpJointVelTrajGenerator::applyVel(const std::array &dq_des, std: if (m_dq_des[i] > m_dq_max[i]) { m_dq_des[i] = m_dq_max[i]; - } else if (m_dq_des[i] < (-m_dq_max[i])) { + } + else if (m_dq_des[i] < (-m_dq_max[i])) { m_dq_des[i] = -m_dq_max[i]; } @@ -442,7 +448,8 @@ void vpJointVelTrajGenerator::applyVel(const std::array &dq_des, std: m_q_final[i] = m_q_max[i] - m_offset_joint_limit; m_delta_q[i] = 0; m_status[i] = FLAGACC; - } else if (m_dq_des[i] < 0) { + } + else if (m_dq_des[i] < 0) { m_delta_q_max[i] = -m_dq_des[i] * m_delta_t; m_sign[i] = -1; m_q_final[i] = m_q_min[i] + m_offset_joint_limit; @@ -456,15 +463,17 @@ void vpJointVelTrajGenerator::applyVel(const std::array &dq_des, std: m_flagSpeed[i] = true; m_status[i] = FLAGDEC; m_delta_q_max[i] = 0; - } else { - // Acceleration or deceleration + } + else { + // Acceleration or deceleration if (m_sign[i] == 1) { if (m_dq_des[i] > m_dq_des_prev[i]) m_status[i] = FLAGACC; else m_status[i] = FLAGDEC; m_delta_q_max[i] = m_dq_des[i] * m_delta_t; - } else { + } + else { if (m_dq_des[i] > m_dq_des_prev[i]) m_status[i] = FLAGDEC; else @@ -517,7 +526,8 @@ void vpJointVelTrajGenerator::applyVel(const std::array &dq_des, std: m_delta_q_max[i] = m_dq_des[i] * m_delta_t; m_sign[i] = 1; m_q_final[i] = m_q_max[i] - m_offset_joint_limit; - } else if (m_dq_des[i] < 0) { + } + else if (m_dq_des[i] < 0) { m_delta_q_max[i] = -m_dq_des[i] * m_delta_t; m_sign[i] = -1; m_q_final[i] = m_q_min[i] + m_offset_joint_limit; @@ -528,12 +538,14 @@ void vpJointVelTrajGenerator::applyVel(const std::array &dq_des, std: int n = (int)(m_delta_q_max[i] / m_delta_q_acc[i]); m_dist_AD[i] = n * (m_delta_q_max[i] - (n + 1) * m_delta_q_acc[i] / 2); } - } else if ((m_delta_q_max[i] > 0) && !m_flagJointLimit) { + } + else if ((m_delta_q_max[i] > 0) && !m_flagJointLimit) { if (m_delta_q_max[i] < (m_delta_q[i] + 2 * m_delta_q_acc[i])) { m_delta_q[i] = m_delta_q_max[i]; m_status[i] = FLAGCTE; - } else if (!m_flagJointLimit) { - /* acceleration moins rapide*/ + } + else if (!m_flagJointLimit) { + /* acceleration moins rapide*/ m_delta_q[i] += (2 * m_delta_q_acc[i]); m_status[i] = FLAGACC; } @@ -603,16 +615,16 @@ std::array vpJointVelTrajGenerator::limitRate(const std::array &desired_values, const std::array &last_desired_values) { - std::array limited_values{}; + std::array limited_values {}; for (size_t i = 0; i < 7; i++) { double desired_difference = (desired_values[i] - last_desired_values[i]) / 1e-3; limited_values[i] = - last_desired_values[i] + std::max(std::min(desired_difference, max_derivatives[i]), -max_derivatives[i]) * 1e-3; + last_desired_values[i] + std::max(std::min(desired_difference, max_derivatives[i]), -max_derivatives[i]) * 1e-3; } return limited_values; } #elif !defined(VISP_BUILD_SHARED_LIBS) // Work around to avoid warning: libvisp_robot.a(vpJointVelTrajGenerator.cpp.o) has no symbols -void dummy_vpJointVelTrajGenerator(){}; +void dummy_vpJointVelTrajGenerator() { }; #endif // VISP_HAVE_FRANKA diff --git a/modules/robot/test/servo-flir-ptu/testRobotFlirPtu.cpp b/modules/robot/test/servo-flir-ptu/testRobotFlirPtu.cpp index 653bf0f8c0..4413a2edc5 100644 --- a/modules/robot/test/servo-flir-ptu/testRobotFlirPtu.cpp +++ b/modules/robot/test/servo-flir-ptu/testRobotFlirPtu.cpp @@ -62,52 +62,56 @@ int main(int argc, char **argv) for (int i = 1; i < argc; i++) { if ((std::string(argv[i]) == "--portname" || std::string(argv[i]) == "-p") && (i + 1 < argc)) { opt_portname = std::string(argv[i + 1]); - } else if ((std::string(argv[i]) == "--baudrate" || std::string(argv[i]) == "-b") && (i + 1 < argc)) { + } + else if ((std::string(argv[i]) == "--baudrate" || std::string(argv[i]) == "-b") && (i + 1 < argc)) { opt_baudrate = std::atoi(argv[i + 1]); - } else if ((std::string(argv[i]) == "--network" || std::string(argv[i]) == "-n")) { + } + else if ((std::string(argv[i]) == "--network" || std::string(argv[i]) == "-n")) { opt_network = true; - } else if ((std::string(argv[i]) == "--reset" || std::string(argv[i]) == "-r")) { + } + else if ((std::string(argv[i]) == "--reset" || std::string(argv[i]) == "-r")) { opt_reset = true; - } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") { + } + else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") { std::cout << "SYNOPSIS" << std::endl - << " " << argv[0] << " [--portname ] [--baudrate ] [--network] [--reset] [--help] [-h]" - << std::endl - << std::endl - << "DESCRIPTION" << std::endl - << " --portname, -p " << std::endl - << " Set serial or tcp port name." << std::endl - << std::endl - << " --baudrate, -b " << std::endl - << " Set serial communication baud rate. Default: " << opt_baudrate << "." << std::endl - << std::endl - << " --network, -n" << std::endl - << " Get PTU network information (Hostname, IP, Gateway) and exit. " << std::endl - << std::endl - << " --reset, -r" << std::endl - << " Reset PTU axis and exit. " << std::endl - << std::endl - << " --help, -h" << std::endl - << " Print this helper message. " << std::endl - << std::endl - << "EXAMPLE" << std::endl - << " - How to get network IP" << std::endl + << " " << argv[0] << " [--portname ] [--baudrate ] [--network] [--reset] [--help] [-h]" + << std::endl + << std::endl + << "DESCRIPTION" << std::endl + << " --portname, -p " << std::endl + << " Set serial or tcp port name." << std::endl + << std::endl + << " --baudrate, -b " << std::endl + << " Set serial communication baud rate. Default: " << opt_baudrate << "." << std::endl + << std::endl + << " --network, -n" << std::endl + << " Get PTU network information (Hostname, IP, Gateway) and exit. " << std::endl + << std::endl + << " --reset, -r" << std::endl + << " Reset PTU axis and exit. " << std::endl + << std::endl + << " --help, -h" << std::endl + << " Print this helper message. " << std::endl + << std::endl + << "EXAMPLE" << std::endl + << " - How to get network IP" << std::endl #ifdef _WIN32 - << " $ " << argv[0] << " -p /dev/ttyUSB0 --network" << std::endl + << " $ " << argv[0] << " -p /dev/ttyUSB0 --network" << std::endl #else - << " $ " << argv[0] << " --portname COM1 --network" << std::endl + << " $ " << argv[0] << " --portname COM1 --network" << std::endl #endif - << " Try to connect FLIR PTU to port: /dev/ttyUSB0 with baudrate: 9600" << std::endl - << " PTU HostName: PTU-5" << std::endl - << " PTU IP : 169.254.110.254" << std::endl - << " PTU Gateway : 0.0.0.0" << std::endl - << " - How to run this binary using serial communication" << std::endl + << " Try to connect FLIR PTU to port: /dev/ttyUSB0 with baudrate: 9600" << std::endl + << " PTU HostName: PTU-5" << std::endl + << " PTU IP : 169.254.110.254" << std::endl + << " PTU Gateway : 0.0.0.0" << std::endl + << " - How to run this binary using serial communication" << std::endl #ifdef _WIN32 - << " $ " << argv[0] << " --portname COM1" << std::endl + << " $ " << argv[0] << " --portname COM1" << std::endl #else - << " $ " << argv[0] << " --portname /dev/ttyUSB0" << std::endl + << " $ " << argv[0] << " --portname /dev/ttyUSB0" << std::endl #endif - << " - How to run this binary using network communication" << std::endl - << " $ " << argv[0] << " --portname tcp:169.254.110.254" << std::endl; + << " - How to run this binary using network communication" << std::endl + << " $ " << argv[0] << " --portname tcp:169.254.110.254" << std::endl; return EXIT_SUCCESS; } @@ -144,12 +148,12 @@ int main(int argc, char **argv) std::cout << "** Test limits getter" << std::endl; std::cout << "Pan pos min/max [deg]: " << vpMath::deg(robot.getPanPosLimits()[0]) << " " - << vpMath::deg(robot.getPanPosLimits()[1]) << std::endl; + << vpMath::deg(robot.getPanPosLimits()[1]) << std::endl; std::cout << "Tilt pos min/max [deg]: " << vpMath::deg(robot.getTiltPosLimits()[0]) << " " - << vpMath::deg(robot.getTiltPosLimits()[1]) << std::endl; + << vpMath::deg(robot.getTiltPosLimits()[1]) << std::endl; std::cout << "Pan/tilt vel max [deg/s]: " << vpMath::deg(robot.getPanTiltVelMax()[0]) << " " - << vpMath::deg(robot.getPanTiltVelMax()[1]) << std::endl - << std::endl; + << vpMath::deg(robot.getPanTiltVelMax()[1]) << std::endl + << std::endl; } { @@ -166,12 +170,12 @@ int main(int argc, char **argv) std::cout << "Modified user min/max limits: " << std::endl; std::cout << "Pan pos min/max [deg]: " << vpMath::deg(robot.getPanPosLimits()[0]) << " " - << vpMath::deg(robot.getPanPosLimits()[1]) << std::endl; + << vpMath::deg(robot.getPanPosLimits()[1]) << std::endl; std::cout << "Tilt pos min/max [deg]: " << vpMath::deg(robot.getTiltPosLimits()[0]) << " " - << vpMath::deg(robot.getTiltPosLimits()[1]) << std::endl; + << vpMath::deg(robot.getTiltPosLimits()[1]) << std::endl; std::cout << "Pan/tilt vel max [deg/s]: " << vpMath::deg(robot.getPanTiltVelMax()[0]) << " " - << vpMath::deg(robot.getPanTiltVelMax()[1]) << std::endl - << std::endl; + << vpMath::deg(robot.getPanTiltVelMax()[1]) << std::endl + << std::endl; } { @@ -185,7 +189,7 @@ int main(int argc, char **argv) { std::cout << "** Test joint positioning" << std::endl; robot.setRobotState(vpRobot::STATE_POSITION_CONTROL); - robot.setMaxRotationVelocity(std::min(robot.getPanTiltVelMax()[0], robot.getPanTiltVelMax()[1]) / + robot.setMaxRotationVelocity(std::min(robot.getPanTiltVelMax()[0], robot.getPanTiltVelMax()[1]) / 2.); // 50% of the slowest axis q = 0; @@ -198,7 +202,7 @@ int main(int argc, char **argv) robot.getPosition(vpRobot::JOINT_STATE, q_mes); std::cout << "Position reached [deg]: " << vpMath::deg(q_mes[0]) << " " << vpMath::deg(q_mes[1]) << std::endl - << std::endl; + << std::endl; } { @@ -214,7 +218,7 @@ int main(int argc, char **argv) robot.getPosition(vpRobot::ARTICULAR_FRAME, q_mes); std::cout << "Position reached [deg]: " << vpMath::deg(q_mes[0]) << " " << vpMath::deg(q_mes[1]) << std::endl - << std::endl; + << std::endl; } { @@ -225,7 +229,7 @@ int main(int argc, char **argv) qdot[1] = vpMath::rad(0); // Tilt velocity in rad/s std::cout << "Set velocity for 4s: " << vpMath::deg(qdot[0]) << " " << vpMath::deg(qdot[1]) << " [deg/s]" - << std::endl; + << std::endl; std::cout << "Enter a caracter to apply" << std::endl; scanf("%d", &answer); @@ -240,8 +244,8 @@ int main(int argc, char **argv) robot.setRobotState(vpRobot::STATE_STOP); robot.getPosition(vpRobot::ARTICULAR_FRAME, q_mes); std::cout << "Position reached: " << vpMath::deg(q_mes[0]) << " " << vpMath::deg(q_mes[1]) << " [deg]" - << std::endl - << std::endl; + << std::endl + << std::endl; } { @@ -252,8 +256,8 @@ int main(int argc, char **argv) v_e[5] = vpMath::rad(5); // wz_e std::cout << "Set cartesian velocity in end-effector frame for 4s: " << v_e[0] << " " << v_e[1] << " " << v_e[2] - << " [m/s] " << vpMath::deg(v_e[3]) << " " << vpMath::deg(v_e[4]) << " " << vpMath::deg(v_e[5]) - << " [deg/s]" << std::endl; + << " [m/s] " << vpMath::deg(v_e[3]) << " " << vpMath::deg(v_e[4]) << " " << vpMath::deg(v_e[5]) + << " [deg/s]" << std::endl; std::cout << "Enter a caracter to apply" << std::endl; scanf("%d", &answer); @@ -269,15 +273,17 @@ int main(int argc, char **argv) robot.setRobotState(vpRobot::STATE_STOP); robot.getPosition(vpRobot::ARTICULAR_FRAME, q_mes); std::cout << "Position reached: " << vpMath::deg(q_mes[0]) << " " << vpMath::deg(q_mes[1]) << " [deg]" - << std::endl - << std::endl; + << std::endl + << std::endl; } std::cout << "** The end" << std::endl; - } catch (const vpRobotException &e) { + } + catch (const vpRobotException &e) { std::cout << "Catch Flir Ptu signal exception: " << e.getMessage() << std::endl; robot.setRobotState(vpRobot::STATE_STOP); - } catch (const vpException &e) { + } + catch (const vpException &e) { std::cout << "Catch Flir Ptu exception: " << e.getMessage() << std::endl; robot.setRobotState(vpRobot::STATE_STOP); } diff --git a/modules/sensor/src/framegrabber/flycapture/vpFlyCaptureGrabber.cpp b/modules/sensor/src/framegrabber/flycapture/vpFlyCaptureGrabber.cpp index a982ee310c..a96ae4e684 100644 --- a/modules/sensor/src/framegrabber/flycapture/vpFlyCaptureGrabber.cpp +++ b/modules/sensor/src/framegrabber/flycapture/vpFlyCaptureGrabber.cpp @@ -184,7 +184,8 @@ FlyCapture2::Camera *vpFlyCaptureGrabber::getCameraHandler() if (m_connected == true) { return &m_camera; - } else { + } + else { return nullptr; } } @@ -451,13 +452,13 @@ void vpFlyCaptureGrabber::setProperty(const FlyCapture2::PropertyType &prop_type prop.absControl = propInfo.absValSupported; switch (prop_value) { case ABS_VALUE: { - float value_ = (std::max)((std::min)((float)value, (float)propInfo.absMax), (float)propInfo.absMin); + float value_ = std::max(std::min((float)value, (float)propInfo.absMax), (float)propInfo.absMin); prop.absValue = value_; break; } case VALUE_A: { unsigned int value_ = - (std::max)((std::min)((unsigned int)value, (unsigned int)propInfo.max), (unsigned int)propInfo.min); + std::max(std::min((unsigned int)value, (unsigned int)propInfo.max), (unsigned int)propInfo.min); prop.valueA = value_; break; } @@ -1341,7 +1342,8 @@ void vpFlyCaptureGrabber::setCameraPower(bool on) if (error == FlyCapture2::PGRERROR_TIMEOUT) { // ignore timeout errors, camera may not be responding to // register reads during power-up - } else if (error != FlyCapture2::PGRERROR_OK) { + } + else if (error != FlyCapture2::PGRERROR_OK) { error.PrintErrorTrace(); throw(vpException(vpException::fatalError, "Cannot power on the camera.")); } @@ -1403,5 +1405,5 @@ vpFlyCaptureGrabber &vpFlyCaptureGrabber::operator>>(vpImage &I) #else // Work around to avoid warning: // libvisp_flycapture.a(vpFlyCaptureGrabber.cpp.o) has no symbols -void dummy_vpFlyCaptureGrabber(){}; +void dummy_vpFlyCaptureGrabber() { }; #endif diff --git a/modules/sensor/src/rgb-depth/occipital_structure/vpOccipitalStructure.cpp b/modules/sensor/src/rgb-depth/occipital_structure/vpOccipitalStructure.cpp index 6e3c05d3d7..d2f06ed1f8 100644 --- a/modules/sensor/src/rgb-depth/occipital_structure/vpOccipitalStructure.cpp +++ b/modules/sensor/src/rgb-depth/occipital_structure/vpOccipitalStructure.cpp @@ -584,7 +584,7 @@ void vpOccipitalStructure::getIMUData(vpColVector *imu_vel, vpColVector *imu_acc } if (ts != nullptr) - *ts = std::max(imu_vel_timestamp, imu_acc_timestamp); + *ts = std::max(imu_vel_timestamp, imu_acc_timestamp); u.unlock(); } diff --git a/modules/sensor/test/rgb-depth/testRealSense2_D435_align.cpp b/modules/sensor/test/rgb-depth/testRealSense2_D435_align.cpp index ac4a01048e..4610f12de3 100644 --- a/modules/sensor/test/rgb-depth/testRealSense2_D435_align.cpp +++ b/modules/sensor/test/rgb-depth/testRealSense2_D435_align.cpp @@ -173,9 +173,9 @@ int main(int argc, char *argv[]) vpImagePoint imPt; vpMeterPixelConversion::convertPoint(cam_projection, x, y, imPt); unsigned int u = - std::min(static_cast(width - 1), static_cast(std::max(0.0, imPt.get_u()))); + std::min(static_cast(width - 1), static_cast(std::max(0.0, imPt.get_u()))); unsigned int v = - std::min(static_cast(height - 1), static_cast(std::max(0.0, imPt.get_v()))); + std::min(static_cast(height - 1), static_cast(std::max(0.0, imPt.get_v()))); I_pcl[v][u] = vpRGBa(pcl_pt.r, pcl_pt.g, pcl_pt.b); } } @@ -195,9 +195,9 @@ int main(int argc, char *argv[]) vpImagePoint imPt; vpMeterPixelConversion::convertPoint(cam_projection, x, y, imPt); unsigned int u = - std::min(static_cast(width - 1), static_cast(std::max(0.0, imPt.get_u()))); + std::min(static_cast(width - 1), static_cast(std::max(0.0, imPt.get_u()))); unsigned int v = - std::min(static_cast(height - 1), static_cast(std::max(0.0, imPt.get_v()))); + std::min(static_cast(height - 1), static_cast(std::max(0.0, imPt.get_v()))); unsigned char depth_viz = getDepthColor(histogram, pcl_pt.z, depth_scale); I_pcl[v][u] = vpRGBa(depth_viz, depth_viz, depth_viz); } @@ -217,9 +217,9 @@ int main(int argc, char *argv[]) vpImagePoint imPt; vpMeterPixelConversion::convertPoint(cam_projection, x, y, imPt); unsigned int u = - std::min(static_cast(width - 1), static_cast(std::max(0.0, imPt.get_u()))); + std::min(static_cast(width - 1), static_cast(std::max(0.0, imPt.get_u()))); unsigned int v = - std::min(static_cast(height - 1), static_cast(std::max(0.0, imPt.get_v()))); + std::min(static_cast(height - 1), static_cast(std::max(0.0, imPt.get_v()))); unsigned char depth_viz = getDepthColor(histogram2, Z, depth_scale); I_pcl2[v][u] = vpRGBa(depth_viz, depth_viz, depth_viz); } diff --git a/modules/sensor/test/rgb-depth/testRealSense2_D435_opencv.cpp b/modules/sensor/test/rgb-depth/testRealSense2_D435_opencv.cpp index f2cc0fe227..06cdf1084f 100644 --- a/modules/sensor/test/rgb-depth/testRealSense2_D435_opencv.cpp +++ b/modules/sensor/test/rgb-depth/testRealSense2_D435_opencv.cpp @@ -225,8 +225,8 @@ int main() vpImagePoint imPt; vpMeterPixelConversion::convertPoint(cam_projection, x, y, imPt); - int u = std::min(static_cast(width - 1), static_cast(std::max(0.0, imPt.get_u()))); - int v = std::min(static_cast(height - 1), static_cast(std::max(0.0, imPt.get_v()))); + int u = std::min(static_cast(width - 1), static_cast(std::max(0.0, imPt.get_u()))); + int v = std::min(static_cast(height - 1), static_cast(std::max(0.0, imPt.get_v()))); unsigned char depth_viz = getDepthColor(histogram, Z, depth_scale); mat_pointcloud.at(v, u) = depth_viz; } diff --git a/modules/sensor/test/rgb-depth/testRealSense_R200.cpp b/modules/sensor/test/rgb-depth/testRealSense_R200.cpp index 7388e378a4..30485d63e8 100644 --- a/modules/sensor/test/rgb-depth/testRealSense_R200.cpp +++ b/modules/sensor/test/rgb-depth/testRealSense_R200.cpp @@ -244,12 +244,12 @@ void test_R200(vpRealSense &rs, const std::map &enables, break; case rs::stream::infrared: - di.init(I_infrared, 0, (int)(std::max)(I_color.getHeight(), I_depth.getHeight()) + 30, "Infrared frame"); + di.init(I_infrared, 0, (int)(std::max(I_color.getHeight(), I_depth.getHeight())) + 30, "Infrared frame"); break; case rs::stream::infrared2: di2.init(I_infrared2, (int)I_infrared.getWidth(), - (int)(std::max)(I_color.getHeight(), I_depth.getHeight()) + 30, "Infrared2 frame"); + (int)(std::max(I_color.getHeight(), I_depth.getHeight())) + 30, "Infrared2 frame"); break; default: diff --git a/modules/sensor/test/rgb-depth/testRealSense_SR300.cpp b/modules/sensor/test/rgb-depth/testRealSense_SR300.cpp index 42bfb80d9b..9e3821bd9d 100644 --- a/modules/sensor/test/rgb-depth/testRealSense_SR300.cpp +++ b/modules/sensor/test/rgb-depth/testRealSense_SR300.cpp @@ -230,7 +230,7 @@ void test_SR300(vpRealSense &rs, const std::map &enables, break; case rs::stream::infrared: - di.init(I_infrared, 0, (int)(std::max)(I_color.getHeight(), I_depth.getHeight()) + 30, "Infrared frame"); + di.init(I_infrared, 0, (int)(std::max(I_color.getHeight(), I_depth.getHeight())) + 30, "Infrared frame"); break; default: diff --git a/modules/tracker/mbt/src/depth/vpMbtFaceDepthDense.cpp b/modules/tracker/mbt/src/depth/vpMbtFaceDepthDense.cpp index c602e14d0d..8a47b5d437 100644 --- a/modules/tracker/mbt/src/depth/vpMbtFaceDepthDense.cpp +++ b/modules/tracker/mbt/src/depth/vpMbtFaceDepthDense.cpp @@ -287,10 +287,10 @@ bool vpMbtFaceDepthDense::computeDesiredFeatures(const vpHomogeneousMatrix &cMo, vpPolygon polygon_2d(roiPts); vpRect bb = polygon_2d.getBoundingBox(); - unsigned int top = (unsigned int)std::max(0.0, bb.getTop()); - unsigned int bottom = (unsigned int)std::min((double)height, std::max(0.0, bb.getBottom())); - unsigned int left = (unsigned int)std::max(0.0, bb.getLeft()); - unsigned int right = (unsigned int)std::min((double)width, std::max(0.0, bb.getRight())); + unsigned int top = (unsigned int)std::max(0.0, bb.getTop()); + unsigned int bottom = (unsigned int)std::min((double)height, std::max(0.0, bb.getBottom())); + unsigned int left = (unsigned int)std::max(0.0, bb.getLeft()); + unsigned int right = (unsigned int)std::min((double)width, std::max(0.0, bb.getRight())); bb.setTop(top); bb.setBottom(bottom); @@ -377,10 +377,10 @@ bool vpMbtFaceDepthDense::computeDesiredFeatures(const vpHomogeneousMatrix &cMo, vpPolygon polygon_2d(roiPts); vpRect bb = polygon_2d.getBoundingBox(); - unsigned int top = (unsigned int)std::max(0.0, bb.getTop()); - unsigned int bottom = (unsigned int)std::min((double)height, std::max(0.0, bb.getBottom())); - unsigned int left = (unsigned int)std::max(0.0, bb.getLeft()); - unsigned int right = (unsigned int)std::min((double)width, std::max(0.0, bb.getRight())); + unsigned int top = (unsigned int)std::max(0.0, bb.getTop()); + unsigned int bottom = (unsigned int)std::min((double)height, std::max(0.0, bb.getBottom())); + unsigned int left = (unsigned int)std::max(0.0, bb.getLeft()); + unsigned int right = (unsigned int)std::min((double)width, std::max(0.0, bb.getRight())); bb.setTop(top); bb.setBottom(bottom); @@ -462,10 +462,10 @@ bool vpMbtFaceDepthDense::computeDesiredFeatures(const vpHomogeneousMatrix &cMo, vpPolygon polygon_2d(roiPts); vpRect bb = polygon_2d.getBoundingBox(); - unsigned int top = (unsigned int)std::max(0.0, bb.getTop()); - unsigned int bottom = (unsigned int)std::min((double)height, std::max(0.0, bb.getBottom())); - unsigned int left = (unsigned int)std::max(0.0, bb.getLeft()); - unsigned int right = (unsigned int)std::min((double)width, std::max(0.0, bb.getRight())); + unsigned int top = (unsigned int)std::max(0.0, bb.getTop()); + unsigned int bottom = (unsigned int)std::min((double)height, std::max(0.0, bb.getBottom())); + unsigned int left = (unsigned int)std::max(0.0, bb.getLeft()); + unsigned int right = (unsigned int)std::min((double)width, std::max(0.0, bb.getRight())); bb.setTop(top); bb.setBottom(bottom); diff --git a/modules/tracker/mbt/src/depth/vpMbtFaceDepthNormal.cpp b/modules/tracker/mbt/src/depth/vpMbtFaceDepthNormal.cpp index b741766410..33238a61bc 100644 --- a/modules/tracker/mbt/src/depth/vpMbtFaceDepthNormal.cpp +++ b/modules/tracker/mbt/src/depth/vpMbtFaceDepthNormal.cpp @@ -184,10 +184,10 @@ bool vpMbtFaceDepthNormal::computeDesiredFeatures(const vpHomogeneousMatrix &cMo vpPolygon polygon_2d(roiPts); vpRect bb = polygon_2d.getBoundingBox(); - unsigned int top = (unsigned int)std::max(0.0, bb.getTop()); - unsigned int bottom = (unsigned int)std::min((double)height, std::max(0.0, bb.getBottom())); - unsigned int left = (unsigned int)std::max(0.0, bb.getLeft()); - unsigned int right = (unsigned int)std::min((double)width, std::max(0.0, bb.getRight())); + unsigned int top = (unsigned int)std::max(0.0, bb.getTop()); + unsigned int bottom = (unsigned int)std::min((double)height, std::max(0.0, bb.getBottom())); + unsigned int left = (unsigned int)std::max(0.0, bb.getLeft()); + unsigned int right = (unsigned int)std::min((double)width, std::max(0.0, bb.getRight())); bb.setTop(top); bb.setBottom(bottom); @@ -350,10 +350,10 @@ bool vpMbtFaceDepthNormal::computeDesiredFeatures(const vpHomogeneousMatrix &cMo vpPolygon polygon_2d(roiPts); vpRect bb = polygon_2d.getBoundingBox(); - unsigned int top = (unsigned int)std::max(0.0, bb.getTop()); - unsigned int bottom = (unsigned int)std::min((double)height, std::max(0.0, bb.getBottom())); - unsigned int left = (unsigned int)std::max(0.0, bb.getLeft()); - unsigned int right = (unsigned int)std::min((double)width, std::max(0.0, bb.getRight())); + unsigned int top = (unsigned int)std::max(0.0, bb.getTop()); + unsigned int bottom = (unsigned int)std::min((double)height, std::max(0.0, bb.getBottom())); + unsigned int left = (unsigned int)std::max(0.0, bb.getLeft()); + unsigned int right = (unsigned int)std::min((double)width, std::max(0.0, bb.getRight())); bb.setTop(top); bb.setBottom(bottom); @@ -511,10 +511,10 @@ bool vpMbtFaceDepthNormal::computeDesiredFeatures(const vpHomogeneousMatrix &cMo vpPolygon polygon_2d(roiPts); vpRect bb = polygon_2d.getBoundingBox(); - unsigned int top = (unsigned int)std::max(0.0, bb.getTop()); - unsigned int bottom = (unsigned int)std::min((double)height, std::max(0.0, bb.getBottom())); - unsigned int left = (unsigned int)std::max(0.0, bb.getLeft()); - unsigned int right = (unsigned int)std::min((double)width, std::max(0.0, bb.getRight())); + unsigned int top = (unsigned int)std::max(0.0, bb.getTop()); + unsigned int bottom = (unsigned int)std::min((double)height, std::max(0.0, bb.getBottom())); + unsigned int left = (unsigned int)std::max(0.0, bb.getLeft()); + unsigned int right = (unsigned int)std::min((double)width, std::max(0.0, bb.getRight())); bb.setTop(top); bb.setBottom(bottom); diff --git a/modules/tracker/mbt/src/edge/vpMbEdgeTracker.cpp b/modules/tracker/mbt/src/edge/vpMbEdgeTracker.cpp index f55f12ab1b..914447f4da 100644 --- a/modules/tracker/mbt/src/edge/vpMbEdgeTracker.cpp +++ b/modules/tracker/mbt/src/edge/vpMbEdgeTracker.cpp @@ -1009,7 +1009,6 @@ void vpMbEdgeTracker::testTracking() // Compare the number of good points with the min between the number of // expected points and number of points that are tracked int nb_min = (int)vpMath::minimum(percentageGdPt * nbExpectedPoint, percentageGdPt * (nbGoodPoint + nbBadPoint)); - // int nb_min = (std::min)(val1, val2); if (nbGoodPoint < nb_min || nbExpectedPoint < 2) { std::ostringstream oss; oss << "Not enough moving edges (" << nbGoodPoint << ") to track the object: expected " << nb_min diff --git a/modules/tracker/mbt/src/edge/vpMbtMeEllipse.cpp b/modules/tracker/mbt/src/edge/vpMbtMeEllipse.cpp index e670dc2744..c0a0a55a93 100644 --- a/modules/tracker/mbt/src/edge/vpMbtMeEllipse.cpp +++ b/modules/tracker/mbt/src/edge/vpMbtMeEllipse.cpp @@ -172,7 +172,7 @@ void vpMbtMeEllipse::computeProjectionError(const vpImage &I, dou } } - sumErrorRad += (std::min)(angle1, angle2); + sumErrorRad += std::min(angle1, angle2); nbFeatures++; } diff --git a/modules/tracker/mbt/src/edge/vpMbtMeLine.cpp b/modules/tracker/mbt/src/edge/vpMbtMeLine.cpp index 86940c13e8..9bbf295f66 100644 --- a/modules/tracker/mbt/src/edge/vpMbtMeLine.cpp +++ b/modules/tracker/mbt/src/edge/vpMbtMeLine.cpp @@ -66,9 +66,8 @@ static void normalizeAngle(double &delta) */ vpMbtMeLine::vpMbtMeLine() : rho(0.), theta(0.), theta_1(M_PI / 2), delta(0.), delta_1(0), sign(1), a(0.), b(0.), c(0.), imin(0), imax(0), - jmin(0), jmax(0), expecteddensity(0.) -{ -} + jmin(0), jmax(0), expecteddensity(0.) +{ } /*! Basic destructor. @@ -120,7 +119,8 @@ void vpMbtMeLine::initTracking(const vpImage &I, const vpImagePoi if (!doNoTrack) vpMeTracker::track(I); - } catch (...) { + } + catch (...) { throw; // throw the original exception } vpCDEBUG(1) << " end vpMeLine::initTracking()" << std::endl; @@ -142,7 +142,7 @@ void vpMbtMeLine::sample(const vpImage &I, bool doNoTrack) // if (me->getSampleStep==0) if (std::fabs(me->getSampleStep()) <= std::numeric_limits::epsilon()) { throw(vpTrackingException(vpTrackingException::fatalError, "Function vpMbtMeLine::sample() called with " - "moving-edges sample step = 0")); + "moving-edges sample step = 0")); } // i, j portions of the line_p @@ -288,14 +288,16 @@ void vpMbtMeLine::seekExtremities(const vpImage &I) if ((P.i < imin) || (P.i > imax) || (P.j < jmin) || (P.j > jmax)) { if (vpDEBUG_ENABLE(3)) vpDisplay::displayCross(I, P.i, P.j, 5, vpColor::cyan); - } else if (!outOfImage(P.i, P.j, (int)(me->getRange() + me->getMaskSize() + 1), (int)rows, (int)cols)) { + } + else if (!outOfImage(P.i, P.j, (int)(me->getRange() + me->getMaskSize() + 1), (int)rows, (int)cols)) { P.track(I, me, false); if (P.getState() == vpMeSite::NO_SUPPRESSION) { list.push_back(P); if (vpDEBUG_ENABLE(3)) vpDisplay::displayCross(I, P.i, P.j, 5, vpColor::green); - } else if (vpDEBUG_ENABLE(3)) + } + else if (vpDEBUG_ENABLE(3)) vpDisplay::displayCross(I, P.i, P.j, 10, vpColor::blue); } } @@ -320,7 +322,8 @@ void vpMbtMeLine::seekExtremities(const vpImage &I) list.push_back(P); if (vpDEBUG_ENABLE(3)) vpDisplay::displayCross(I, P.i, P.j, 5, vpColor::green); - } else if (vpDEBUG_ENABLE(3)) + } + else if (vpDEBUG_ENABLE(3)) vpDisplay::displayCross(I, P.i, P.j, 10, vpColor::blue); } } @@ -434,26 +437,15 @@ void vpMbtMeLine::computeProjectionError(const vpImage &_I, doubl vpDisplay::displayArrow(_I, it->get_i(), it->get_j(), (int)(it->get_i() + length * cos(angle)), (int)(it->get_j() + length * sin(angle)), vpColor::red, length >= 20 ? length / 5 : 4, length >= 20 ? length / 10 : 2, thickness); - } else { + } + else { vpDisplay::displayArrow(_I, it->get_i(), it->get_j(), (int)(it->get_i() + length * cos(angle + M_PI)), (int)(it->get_j() + length * sin(angle + M_PI)), vpColor::red, length >= 20 ? length / 5 : 4, length >= 20 ? length / 10 : 2, thickness); } } - // double angle1 = sqrt(vpMath::sqr(deltaNormalized-angle)); - // double angle2 = sqrt(vpMath::sqr(deltaNormalized- - // (angle-M_PI))); - - _sumErrorRad += (std::min)(angle1, angle2); - - // if(std::fabs(deltaNormalized-angle) > M_PI / 2) - // { - // sumErrorRad += sqrt(vpMath::sqr(deltaNormalized-angle)) - M_PI - // / 2; - // } else { - // sumErrorRad += sqrt(vpMath::sqr(deltaNormalized-angle)); - // } + _sumErrorRad += std::min(angle1, angle2); _nbFeatures++; } @@ -565,7 +557,8 @@ void vpMbtMeLine::track(const vpImage &I) // Expected density could be modified if some vpMeSite are no more tracked because they are outside the mask. expecteddensity = (double)list.size(); } - } catch (...) { + } + catch (...) { throw; // throw the original exception } } @@ -693,11 +686,9 @@ void vpMbtMeLine::bubbleSortI() { #if 0 unsigned int nbElmt = list.size(); - for (unsigned int pass = 1; pass < nbElmt; pass++) - { + for (unsigned int pass = 1; pass < nbElmt; pass++) { list.front(); - for (unsigned int i=0; i < nbElmt-pass; i++) - { + for (unsigned int i = 0; i < nbElmt-pass; i++) { vpMeSite s1 = list.value(); vpMeSite s2 = list.nextValue(); if (s1.ifloat > s2.ifloat) @@ -716,11 +707,9 @@ void vpMbtMeLine::bubbleSortJ() { #if 0 unsigned int nbElmt = list.size(); - for(unsigned int pass=1; pass < nbElmt; pass++) - { + for (unsigned int pass = 1; pass < nbElmt; pass++) { list.front(); - for (unsigned int i=0; i < nbElmt-pass; i++) - { + for (unsigned int i = 0; i < nbElmt-pass; i++) { vpMeSite s1 = list.value(); vpMeSite s2 = list.nextValue(); if (s1.jfloat > s2.jfloat) diff --git a/modules/tracker/mbt/src/vpMbScanLine.cpp b/modules/tracker/mbt/src/vpMbScanLine.cpp index 3ace72ea3f..e8c7b6275f 100644 --- a/modules/tracker/mbt/src/vpMbScanLine.cpp +++ b/modules/tracker/mbt/src/vpMbScanLine.cpp @@ -61,8 +61,8 @@ vpMbScanLine::vpMbScanLine() : w(0), h(0), K(), maskBorder(0), mask(), primitive_ids(), visibility_samples(), depthTreshold(1e-06) #if defined(DEBUG_DISP) - , - dispMaskDebug(nullptr), dispLineDebug(nullptr), linedebugImg() + , + dispMaskDebug(nullptr), dispLineDebug(nullptr), linedebugImg() #endif { #if defined(VISP_HAVE_X11) && defined(DEBUG_DISP) @@ -112,8 +112,8 @@ void vpMbScanLine::drawLineY(const vpColVector &a, const vpColVector &b, const v if (y0 >= h - 1 || y1 < 0 || std::fabs(y1 - y0) <= std::numeric_limits::epsilon()) return; - const unsigned int _y0 = (std::max)((unsigned int)0, (unsigned int)(std::ceil(y0))); - const double _y1 = (std::min)((double)h, (double)y1); + const unsigned int _y0 = std::max((unsigned int)0, (unsigned int)(std::ceil(y0))); + const double _y1 = std::min((double)h, (double)y1); const bool b_sample_Y = (std::fabs(y0 - y1) > std::fabs(x0 - x1)); @@ -161,8 +161,8 @@ void vpMbScanLine::drawLineX(const vpColVector &a, const vpColVector &b, const v if (x0 >= w - 1 || x1 < 0 || std::fabs(x1 - x0) <= std::numeric_limits::epsilon()) return; - const unsigned int _x0 = (std::max)((unsigned int)0, (unsigned int)(std::ceil(x0))); - const double _x1 = (std::min)((double)w, (double)x1); + const unsigned int _x0 = std::max((unsigned int)0, (unsigned int)(std::ceil(x0))); + const double _x1 = std::min((double)w, (double)x1); const bool b_sample_Y = (std::fabs(y0 - y1) > std::fabs(x0 - x1)); @@ -280,7 +280,8 @@ void vpMbScanLine::createScanLinesFromLocals(std::vector((unsigned int)0, (unsigned int)(std::ceil(last_visible.p))); + double x1 = std::min((double)w, (double)s.p); for (unsigned int x = x0 + maskBorder; x < x1 - maskBorder; ++x) { primitive_ids[(unsigned int)y][(unsigned int)x] = last_visible.ID; @@ -461,8 +462,8 @@ void vpMbScanLine::drawScene(const std::vector((unsigned int)0, (unsigned int)(std::ceil(last_visible.p))); + double y1 = std::min((double)h, (double)s.p); for (unsigned int y = y0 + maskBorder; y < y1 - maskBorder; ++y) { // primitive_ids[(unsigned int)y][(unsigned int)x] = // last_visible.ID; @@ -585,7 +586,8 @@ void vpMbScanLine::queryLineVisibility(const vpPoint &a, const vpPoint &b, // Instead of swap we set the right address of the corresponding pointers a_ = b; b_ = a; - } else { + } + else { a_ = a; b_ = b; } @@ -594,8 +596,8 @@ void vpMbScanLine::queryLineVisibility(const vpPoint &a, const vpPoint &b, if (*v0 >= size - 1 || *v1 < 0 || std::fabs(*v1 - *v0) <= std::numeric_limits::epsilon()) return; - const int _v0 = (std::max)(0, int(std::ceil(*v0))); - const int _v1 = (std::min)((int)(size - 1), (int)(std::ceil(*v1) - 1)); + const int _v0 = std::max(0, int(std::ceil(*v0))); + const int _v1 = std::min((int)(size - 1), (int)(std::ceil(*v1) - 1)); const std::set &visible_samples = visibility_samples[edge]; int last = _v0; @@ -617,13 +619,15 @@ void vpMbScanLine::queryLineVisibility(const vpPoint &a, const vpPoint &b, line_start = a_; line_end = p; b_line_started = true; - } else if (v == _v1) { - // line_end = b; + } + else if (v == _v1) { + // line_end = b; line_end = b_; if (!b_line_started) line_start = p; b_line_started = true; - } else { + } + else { line_end = p; if (!b_line_started) line_start = p; @@ -676,7 +680,8 @@ vpMbScanLine::vpMbScanLineEdge vpMbScanLine::makeMbScanLineEdge(const vpPoint &a if (_a[i] < _b[i]) { b_comp = true; break; - } else if (_a[i] > _b[i]) + } + else if (_a[i] > _b[i]) break; if (b_comp) @@ -718,8 +723,8 @@ double vpMbScanLine::getAlpha(double x, double X0, double Z0, double X1, double if (vpMath::isNaN(alpha) || vpMath::isInf(alpha)) return 0.0; - alpha = (std::min)(1.0, alpha); - alpha = (std::max)(0.0, alpha); + alpha = std::min(1.0, alpha); + alpha = std::max(0.0, alpha); return alpha; } diff --git a/modules/tracker/me/src/moving-edges/vpMeLine.cpp b/modules/tracker/me/src/moving-edges/vpMeLine.cpp index db84354203..737597f8ae 100644 --- a/modules/tracker/me/src/moving-edges/vpMeLine.cpp +++ b/modules/tracker/me/src/moving-edges/vpMeLine.cpp @@ -752,9 +752,9 @@ void vpMeLine::computeRhoTheta(const vpImage &I) double co_rho_lim1 = fabs(((double)(height_ - i)) / cos(theta)); double co_rho_lim2 = fabs(((double)(width_ - j)) / sin(theta)); - double rho_lim = (std::min)(rho_lim1, rho_lim2); - double co_rho_lim = (std::min)(co_rho_lim1, co_rho_lim2); - incr = (int)std::floor((std::min)(rho_lim, co_rho_lim)); + double rho_lim = std::min(rho_lim1, rho_lim2); + double co_rho_lim = std::min(co_rho_lim1, co_rho_lim2); + incr = (int)std::floor(std::min(rho_lim, co_rho_lim)); if (incr < INCR_MIN) { vpERROR_TRACE("increment is too small"); throw(vpTrackingException(vpTrackingException::fatalError, "increment is too small")); diff --git a/modules/vision/src/key-point/vpKeyPoint.cpp b/modules/vision/src/key-point/vpKeyPoint.cpp index 83d87ea345..b0e6d5bf20 100644 --- a/modules/vision/src/key-point/vpKeyPoint.cpp +++ b/modules/vision/src/key-point/vpKeyPoint.cpp @@ -858,7 +858,7 @@ void vpKeyPoint::createImageMatching(vpImage &IRef, vpImage(IRef.getHeight(), ICurrent.getHeight()); IMatching = vpImage(height, width); } @@ -868,7 +868,7 @@ void vpKeyPoint::createImageMatching(vpImage &IRef, vpImage(IRef.getHeight(), ICurrent.getHeight()); IMatching = vpImage(height, width); } diff --git a/modules/vision/src/plane-estimation/vpPlaneEstimation.cpp b/modules/vision/src/plane-estimation/vpPlaneEstimation.cpp index 8fa0e6b070..1c9c13e78c 100644 --- a/modules/vision/src/plane-estimation/vpPlaneEstimation.cpp +++ b/modules/vision/src/plane-estimation/vpPlaneEstimation.cpp @@ -235,10 +235,10 @@ vpPlaneEstimation::estimatePlane(const vpImage &I_depth_raw, double de // Limit research area const auto roi_bb = roi.getBoundingBox(); - const int roi_top = static_cast(std::max(0., roi_bb.getTop())); - const int roi_bottom = static_cast(std::min(static_cast(I_depth_raw.getHeight()), roi_bb.getBottom())); - const int roi_left = static_cast(std::max(0., roi_bb.getLeft())); - const int roi_right = static_cast(std::min(static_cast(I_depth_raw.getWidth()), roi_bb.getRight())); + const int roi_top = static_cast(std::max(0., roi_bb.getTop())); + const int roi_bottom = static_cast(std::min(static_cast(I_depth_raw.getHeight()), roi_bb.getBottom())); + const int roi_left = static_cast(std::max(0., roi_bb.getLeft())); + const int roi_right = static_cast(std::min(static_cast(I_depth_raw.getWidth()), roi_bb.getRight())); // Reduce computation time by using subsample factor unsigned int subsample_factor = @@ -286,8 +286,8 @@ vpPlaneEstimation::estimatePlane(const vpImage &I_depth_raw, double de vpImagePoint ip {}; vpMeterPixelConversion::convertPoint(depth_intrinsics, X / Z, Y / Z, ip); - const int b = static_cast(std::max(0., 255 * (1 - 2 * weights[i]))); - const int r = static_cast(std::max(0., 255 * (2 * weights[i] - 1))); + const int b = static_cast(std::max(0., 255 * (1 - 2 * weights[i]))); + const int r = static_cast(std::max(0., 255 * (2 * weights[i] - 1))); const int g = 255 - b - r; heat_map->get()[static_cast(ip.get_i())][static_cast(ip.get_j())] = vpColor(r, g, b); diff --git a/modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.cpp b/modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.cpp index 0a72429687..2463d69225 100644 --- a/modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.cpp +++ b/modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.cpp @@ -359,7 +359,7 @@ double pythag(double a, double b) return (pyth); } - r = ((std::min)(std::fabs(a), std::fabs(b)) / p) * ((std::min)(std::fabs(a), std::fabs(b)) / p); + 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) diff --git a/modules/vision/src/pose-estimation/vpPoseDementhon.cpp b/modules/vision/src/pose-estimation/vpPoseDementhon.cpp index 9d600101aa..2a60c93ba9 100644 --- a/modules/vision/src/pose-estimation/vpPoseDementhon.cpp +++ b/modules/vision/src/pose-estimation/vpPoseDementhon.cpp @@ -454,7 +454,7 @@ void vpPose::poseDementhonPlan(vpHomogeneousMatrix &cMo) vpMatrix Ap, imA, imAt, kAt; bool isRankEqualTo3 = false; // Indicates if the rank of A is the expected one double logNofSvdThresh = std::log(m_dementhonSvThresh)/lnOfSvdFactorUsed; // Get the log_n(m_dementhonSvThresh), where n is the factor by which we will multiply it if the svd decomposition fails. - int nbMaxIter = static_cast(std::max(std::ceil(logNOfSvdThresholdLimit - logNofSvdThresh), 1.)); // Ensure that if the user chose a threshold > svdThresholdLimit, at least 1 iteration of svd decomposition is performed + int nbMaxIter = static_cast(std::max(std::ceil(logNOfSvdThresholdLimit - logNofSvdThresh), 1.)); // Ensure that if the user chose a threshold > svdThresholdLimit, at least 1 iteration of svd decomposition is performed double svdThreshold = m_dementhonSvThresh; int irank = 0; for (int i = 0; i < nbMaxIter && !isRankEqualTo3; i++) { diff --git a/modules/vision/src/pose-estimation/vpPoseRGBD.cpp b/modules/vision/src/pose-estimation/vpPoseRGBD.cpp index 3dbf651bf0..260855c0fd 100644 --- a/modules/vision/src/pose-estimation/vpPoseRGBD.cpp +++ b/modules/vision/src/pose-estimation/vpPoseRGBD.cpp @@ -165,12 +165,12 @@ bool vpPose::computePlanarObjectPoseFromRGBD(const vpImage &depthMap, con vpPolygon polygon(corners); vpRect bb = polygon.getBoundingBox(); - unsigned int top = static_cast(std::max(0, static_cast(bb.getTop()))); + unsigned int top = static_cast(std::max(0, static_cast(bb.getTop()))); unsigned int bottom = - static_cast(std::min(static_cast(depthMap.getHeight()) - 1, static_cast(bb.getBottom()))); - unsigned int left = static_cast(std::max(0, static_cast(bb.getLeft()))); + static_cast(std::min(static_cast(depthMap.getHeight()) - 1, static_cast(bb.getBottom()))); + unsigned int left = static_cast(std::max(0, static_cast(bb.getLeft()))); unsigned int right = - static_cast(std::min(static_cast(depthMap.getWidth()) - 1, static_cast(bb.getRight()))); + static_cast(std::min(static_cast(depthMap.getWidth()) - 1, static_cast(bb.getRight()))); std::vector points_3d; points_3d.reserve((bottom - top) * (right - left)); @@ -224,7 +224,7 @@ bool vpPose::computePlanarObjectPoseFromRGBD(const vpImage &depthMap, con pose.addPoints(pose_points); if (pose.computePose(vpPose::VIRTUAL_VS, cMo)) { if (confidence_index != nullptr) { - *confidence_index = std::min(1.0, normalized_weights * static_cast(nb_points_3d) / polygon.getArea()); + *confidence_index = std::min(1.0, normalized_weights * static_cast(nb_points_3d) / polygon.getArea()); } return true; } @@ -277,12 +277,12 @@ bool vpPose::computePlanarObjectPoseFromRGBD(const vpImage &depthMap, // The area to calculate final confidence index should be total area of the tags totalArea += polygon.getArea(); - unsigned int top = static_cast(std::max(0, static_cast(bb.getTop()))); + unsigned int top = static_cast(std::max(0, static_cast(bb.getTop()))); unsigned int bottom = static_cast( - std::min(static_cast(depthMap.getHeight()) - 1, static_cast(bb.getBottom()))); - unsigned int left = static_cast(std::max(0, static_cast(bb.getLeft()))); + std::min(static_cast(depthMap.getHeight()) - 1, static_cast(bb.getBottom()))); + unsigned int left = static_cast(std::max(0, static_cast(bb.getLeft()))); unsigned int right = - static_cast(std::min(static_cast(depthMap.getWidth()) - 1, static_cast(bb.getRight()))); + 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++) { @@ -419,7 +419,7 @@ bool vpPose::computePlanarObjectPoseFromRGBD(const vpImage &depthMap, pose.addPoints(pose_points); if (pose.computePose(vpPose::VIRTUAL_VS, cMo)) { if (confidence_index != nullptr) { - *confidence_index = std::min(1.0, normalized_weights * static_cast(nb_points_3d) / totalArea); + *confidence_index = std::min(1.0, normalized_weights * static_cast(nb_points_3d) / totalArea); } return true; } diff --git a/modules/vision/src/pose-estimation/vpPoseRansac.cpp b/modules/vision/src/pose-estimation/vpPoseRansac.cpp index 7008fe4832..2a746200d8 100644 --- a/modules/vision/src/pose-estimation/vpPoseRansac.cpp +++ b/modules/vision/src/pose-estimation/vpPoseRansac.cpp @@ -494,10 +494,10 @@ bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneo int vpPose::computeRansacIterations(double probability, double epsilon, const int sampleSize, int maxIterations) { - probability = (std::max)(probability, 0.0); - probability = (std::min)(probability, 1.0); - epsilon = (std::max)(epsilon, 0.0); - epsilon = (std::min)(epsilon, 1.0); + probability = std::max(probability, 0.0); + probability = std::min(probability, 1.0); + epsilon = std::max(epsilon, 0.0); + epsilon = std::min(epsilon, 1.0); if (vpMath::nul(epsilon)) { // no outliers @@ -522,7 +522,7 @@ int vpPose::computeRansacIterations(double probability, double epsilon, const in return 0; } - N = log((std::max)(1.0 - probability, std::numeric_limits::epsilon())) / logval; + N = log(std::max(1.0 - probability, std::numeric_limits::epsilon())) / logval; if (logval < 0.0 && N < maxIterations) { return (int)ceil(N); } diff --git a/tutorial/image/tutorial-canny.cpp b/tutorial/image/tutorial-canny.cpp index c5ece751ae..f11301bf5e 100644 --- a/tutorial/image/tutorial-canny.cpp +++ b/tutorial/image/tutorial-canny.cpp @@ -53,7 +53,7 @@ void computeMeanMaxStdev(const vpImage &I, float &mean, float &max, float &st for (unsigned int r = 0; r < nbRows; r++) { for (unsigned int c = 0; c < nbCols; c++) { mean += I[r][c]; - max = std::max(max, static_cast(I[r][c])); + max = std::max(max, static_cast(I[r][c])); } } mean *= scale; diff --git a/tutorial/imgproc/flood-fill/tutorial-flood-fill.cpp b/tutorial/imgproc/flood-fill/tutorial-flood-fill.cpp index aee4d47371..1286b8835f 100644 --- a/tutorial/imgproc/flood-fill/tutorial-flood-fill.cpp +++ b/tutorial/imgproc/flood-fill/tutorial-flood-fill.cpp @@ -111,25 +111,32 @@ int getOctant(const vpImagePoint &imPt1, const vpImagePoint &imPt2) if (dx >= 0 && dy >= 0) { if (dy >= dx) { return 1; - } else { + } + else { return 0; } - } else if (dx < 0 && dy >= 0) { + } + else if (dx < 0 && dy >= 0) { if (-dx >= dy) { return 3; - } else { + } + else { return 2; } - } else if (dx < 0 && dy < 0) { + } + else if (dx < 0 && dy < 0) { if (dy <= dx) { return 5; - } else { + } + else { return 4; } - } else { + } + else { if (dx >= -dy) { return 7; - } else { + } + else { return 6; } } @@ -154,8 +161,8 @@ void drawLine(vpImage &I, const unsigned char value, const vpImag vpImagePoint currentPt(y, x); currentPt = switchFromOctantZeroTo(octant, currentPt); - unsigned int i = std::min(I.getHeight() - 1, (unsigned int)std::max(0.0, currentPt.get_i())); - unsigned int j = std::min(I.getWidth() - 1, (unsigned int)std::max(0.0, currentPt.get_j())); + unsigned int i = std::min(I.getHeight() - 1, (unsigned int)std::max(0.0, currentPt.get_i())); + unsigned int j = std::min(I.getWidth() - 1, (unsigned int)std::max(0.0, currentPt.get_j())); I[i][j] = value; if (D >= 0) { @@ -196,7 +203,7 @@ int main() vpDisplay::display(I); std::stringstream ss; ss << "Left click to draw polygon " << i + 1 << "/3" - << ", right click to close the shape."; + << ", right click to close the shape."; vpDisplay::displayText(I, 20, 20, ss.str(), vpColor::red); vpDisplay::flush(I); diff --git a/tutorial/tracking/dnn/tutorial-megapose-live-single-object-tracking.cpp b/tutorial/tracking/dnn/tutorial-megapose-live-single-object-tracking.cpp index 0fff291973..14ba48ae7f 100644 --- a/tutorial/tracking/dnn/tutorial-megapose-live-single-object-tracking.cpp +++ b/tutorial/tracking/dnn/tutorial-megapose-live-single-object-tracking.cpp @@ -434,7 +434,7 @@ int main(int argc, const char *argv[]) } const double frameEnd = vpTime::measureTimeMs(); if (!isLiveCapture) { - vpTime::wait(std::max(0.0, videoFrametime - (frameEnd - frameStart))); + vpTime::wait(std::max(0.0, videoFrametime - (frameEnd - frameStart))); } frameTimes.push_back(vpTime::measureTimeMs() - frameStart); } From 477471481ece7b06d00f654dda8c6bbfdf749f5e Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Fri, 5 Jan 2024 18:55:26 +0100 Subject: [PATCH 3/4] Attempt to fix previous commit --- modules/core/src/math/matrix/vpMatrix_qr.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/modules/core/src/math/matrix/vpMatrix_qr.cpp b/modules/core/src/math/matrix/vpMatrix_qr.cpp index fcc5e9b97e..8140353fce 100644 --- a/modules/core/src/math/matrix/vpMatrix_qr.cpp +++ b/modules/core/src/math/matrix/vpMatrix_qr.cpp @@ -560,7 +560,7 @@ unsigned int vpMatrix::qr(vpMatrix &Q, vpMatrix &R, bool full, bool squareR, dou integer dimWork = -1; double *qrdata = new double[m * na]; - double *tau = new double[std::min(m, q)]; + double *tau = new double[(std::min(m, q))]; double *work = new double[1]; integer info; @@ -842,7 +842,7 @@ unsigned int vpMatrix::qrPivot(vpMatrix &Q, vpMatrix &R, vpMatrix &P, bool full, #else integer m = (integer)rowNum; // also rows of Q integer n = (integer)colNum; // also columns of R - integer r = >(n, m); // a priori non-null rows of R = rank of R + integer r = std::min(n, m); // a priori non-null rows of R = rank of R integer q = r; // columns of Q and rows of R integer na = n; // columns of A From c0be82a47b869a28d3107efbcc766caf990102b0 Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Fri, 5 Jan 2024 19:50:31 +0100 Subject: [PATCH 4/4] Attempt to fix previous commit --- modules/core/src/math/matrix/vpMatrix_qr.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/core/src/math/matrix/vpMatrix_qr.cpp b/modules/core/src/math/matrix/vpMatrix_qr.cpp index 8140353fce..3a76d556e3 100644 --- a/modules/core/src/math/matrix/vpMatrix_qr.cpp +++ b/modules/core/src/math/matrix/vpMatrix_qr.cpp @@ -560,7 +560,7 @@ unsigned int vpMatrix::qr(vpMatrix &Q, vpMatrix &R, bool full, bool squareR, dou integer dimWork = -1; double *qrdata = new double[m * na]; - double *tau = new double[(std::min(m, q))]; + double *tau = new double[std::min(m, q)]; double *work = new double[1]; integer info;