From c1b629f88adb2c9ac455472b74c0c1dcecbb93c6 Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Thu, 4 Jul 2024 22:43:33 +0200 Subject: [PATCH] Fix misra c++ issues --- .../core/include/visp3/core/vpImageFilter.h | 84 +- .../core/include/visp3/core/vpImageTools.h | 27 +- .../include/visp3/core/vpImageTools_warp.h | 26 +- .../visp3/imgproc/vpCircleHoughTransform.h | 26 + .../include/visp3/imgproc/vpContours.h | 4 +- .../imgproc/src/vpCircleHoughTransform.cpp | 1210 ----------------- .../src/vpCircleHoughTransform_centers.cpp | 569 ++++++++ .../src/vpCircleHoughTransform_circles.cpp | 387 ++++++ .../src/vpCircleHoughTransform_common.cpp | 419 ++++++ .../private/vpLevenbergMarquartd.cpp | 52 +- modules/vision/src/pose-estimation/vpPose.cpp | 13 +- .../src/pose-estimation/vpPoseDementhon.cpp | 12 +- .../src/pose-estimation/vpPoseLagrange.cpp | 17 +- .../vision/src/pose-estimation/vpPoseRGBD.cpp | 3 +- .../src/pose-estimation/vpPoseRansac.cpp | 12 +- 15 files changed, 1552 insertions(+), 1309 deletions(-) delete mode 100644 modules/imgproc/src/vpCircleHoughTransform.cpp create mode 100644 modules/imgproc/src/vpCircleHoughTransform_centers.cpp create mode 100644 modules/imgproc/src/vpCircleHoughTransform_circles.cpp create mode 100644 modules/imgproc/src/vpCircleHoughTransform_common.cpp diff --git a/modules/core/include/visp3/core/vpImageFilter.h b/modules/core/include/visp3/core/vpImageFilter.h index f373011ff9..4039856615 100644 --- a/modules/core/include/visp3/core/vpImageFilter.h +++ b/modules/core/include/visp3/core/vpImageFilter.h @@ -204,21 +204,22 @@ class VISP_EXPORT vpImageFilter // between 0 and 255 for an vpImage float scaleX = 1.f; float scaleY = 1.f; + const unsigned int val2 = 2U; if (filteringType == CANNY_GBLUR_SOBEL_FILTERING) { if (computeDx) { - scaleX = static_cast(vpImageFilter::getSobelKernelX(gradientFilterX.data, (apertureGradient - 1) / 2)); + scaleX = static_cast(vpImageFilter::getSobelKernelX(gradientFilterX.data, (apertureGradient - 1) / val2)); } if (computeDy) { - scaleY = static_cast(vpImageFilter::getSobelKernelY(gradientFilterY.data, (apertureGradient - 1) / 2)); + scaleY = static_cast(vpImageFilter::getSobelKernelY(gradientFilterY.data, (apertureGradient - 1) / val2)); } } else if (filteringType == CANNY_GBLUR_SCHARR_FILTERING) { if (computeDx) { - scaleX = static_cast(vpImageFilter::getScharrKernelX(gradientFilterX.data, (apertureGradient - 1) / 2)); + scaleX = static_cast(vpImageFilter::getScharrKernelX(gradientFilterX.data, (apertureGradient - 1) / val2)); } if (computeDy) { - scaleY = static_cast(vpImageFilter::getScharrKernelY(gradientFilterY.data, (apertureGradient - 1) / 2)); + scaleY = static_cast(vpImageFilter::getScharrKernelY(gradientFilterY.data, (apertureGradient - 1) / val2)); } } @@ -375,13 +376,16 @@ class VISP_EXPORT vpImageFilter float accu = 0; float t = upperThresholdRatio * totalNbPixels; float bon = 0; - for (unsigned int i = 0; i < nbBins; ++i) { + unsigned int i = 0; + bool notFound = true; + while ((i < nbBins) && notFound) { float tf = static_cast(hist[i]); accu = accu + tf; if (accu > t) { bon = static_cast(i); - break; + notFound = false; } + ++i; } float upperThresh = std::max(bon, 1.f); lowerThresh = lowerThresholdRatio * bon; @@ -397,8 +401,9 @@ class VISP_EXPORT vpImageFilter */ template static double derivativeFilterX(const vpImage &I, unsigned int r, unsigned int c) { - return ((2047.0 * static_cast(I[r][c + 1] - I[r][c - 1])) + (913.0 * static_cast(I[r][c + 2] - I[r][c - 2])) + - (112.0 * static_cast(I[r][c + 3] - I[r][c - 3]))) / 8418.0; + const int val1 = 1, val2 = 2, val3 = 3; + return ((2047.0 * static_cast(I[r][c + val1] - I[r][c - val1])) + (913.0 * static_cast(I[r][c + val2] - I[r][c - val2])) + + (112.0 * static_cast(I[r][c + val3] - I[r][c - val3]))) / 8418.0; } /*! @@ -410,8 +415,9 @@ class VISP_EXPORT vpImageFilter */ template static double derivativeFilterY(const vpImage &I, unsigned int r, unsigned int c) { - return ((2047.0 * static_cast(I[r + 1][c] - I[r - 1][c])) + (913.0 * static_cast(I[r + 2][c] - I[r - 2][c])) + - (112.0 * static_cast(I[r + 3][c] - I[r - 3][c]))) / 8418.0; + const int val1 = 1, val2 = 2, val3 = 3; + return ((2047.0 * static_cast(I[r + val1][c] - I[r - val1][c])) + (913.0 * static_cast(I[r + val2][c] - I[r - val2][c])) + + (112.0 * static_cast(I[r + val3][c] - I[r - val3][c]))) / 8418.0; } /*! @@ -674,11 +680,13 @@ class VISP_EXPORT vpImageFilter static inline unsigned char filterGaussXPyramidal(const vpImage &I, unsigned int i, unsigned int j) { - return static_cast(((1. * I[i][j - 2]) + (4. * I[i][j - 1]) + (6. * I[i][j]) + (4. * I[i][j + 1]) + (1. * I[i][j + 2])) / 16.); + const int val2 = 2; + return static_cast(((1. * I[i][j - val2]) + (4. * I[i][j - 1]) + (6. * I[i][j]) + (4. * I[i][j + 1]) + (1. * I[i][j + val2])) / 16.); } static inline unsigned char filterGaussYPyramidal(const vpImage &I, unsigned int i, unsigned int j) { - return static_cast(((1. * I[i - 2][j]) + (4. * I[i - 1][j]) + (6. * I[i][j]) + (4. * I[i + 1][j]) + (1. * I[i + 2][j])) / 16.); + const int val2 = 2; + return static_cast(((1. * I[i - val2][j]) + (4. * I[i - 1][j]) + (6. * I[i][j]) + (4. * I[i + 1][j]) + (1. * I[i + val2][j])) / 16.); } template @@ -774,13 +782,14 @@ class VISP_EXPORT vpImageFilter const unsigned int stop = (size - 1) / 2; const unsigned int width = I.getWidth(); FilterType result = static_cast(0.); + const unsigned int twice = 2; for (unsigned int i = 1; i <= stop; ++i) { if ((c + i) < width) { result += filter[i] * static_cast(I[r][c + i] + I[r][c - i]); } else { - result += filter[i] * static_cast(I[r][((2 * width) - c) - i - 1] + I[r][c - i]); + result += filter[i] * static_cast(I[r][((twice * width) - c) - i - 1] + I[r][c - i]); } } return result + (filter[0] * static_cast(I[r][c])); @@ -882,13 +891,13 @@ class VISP_EXPORT vpImageFilter const unsigned int height = I.getHeight(); const unsigned int stop = (size - 1) / 2; FilterType result = static_cast(0.); - + const unsigned int twiceHeight = 2 * height; for (unsigned int i = 1; i <= stop; ++i) { if ((r + i) < height) { result += filter[i] * static_cast(I[r + i][c] + I[r - i][c]); } else { - result += filter[i] * static_cast(I[((2 * height) - r) - i - 1][c] + I[r - i][c]); + result += filter[i] * static_cast(I[(twiceHeight - r) - i - 1][c] + I[r - i][c]); } } return result + (filter[0] * static_cast(I[r][c])); @@ -934,13 +943,14 @@ class VISP_EXPORT vpImageFilter */ template static double gaussianFilter(const vpImage &fr, unsigned int r, unsigned int c) { + const int val2 = 2; return ((15.0 * fr[r][c]) + (12.0 * (fr[r - 1][c] + fr[r][c - 1] + fr[r + 1][c] + fr[r][c + 1])) + (9.0 * (fr[r - 1][c - 1] + fr[r + 1][c - 1] + fr[r - 1][c + 1] + fr[r + 1][c + 1])) + - (5.0 * (fr[r - 2][c] + fr[r][c - 2] + fr[r + 2][c] + fr[r][c + 2])) + - (4.0 * (fr[r - 2][c + 1] + fr[r - 2][c - 1] + fr[r - 1][c - 2] + fr[r + 1][c - 2] + fr[r + 2][c - 1] + - fr[r + 2][c + 1] + fr[r - 1][c + 2] + fr[r + 1][c + 2])) + - (2.0 * (fr[r - 2][c - 2] + fr[r + 2][c - 2] + fr[r - 2][c + 2] + fr[r + 2][c + 2]))) / 159.0; + (5.0 * (fr[r - val2][c] + fr[r][c - val2] + fr[r + val2][c] + fr[r][c + val2])) + + (4.0 * (fr[r - val2][c + 1] + fr[r - val2][c - 1] + fr[r - 1][c - val2] + fr[r + 1][c - val2] + fr[r + val2][c - 1] + + fr[r + val2][c + 1] + fr[r - 1][c + val2] + fr[r + 1][c + val2])) + + (2.0 * (fr[r - val2][c - val2] + fr[r + val2][c - val2] + fr[r - val2][c + val2] + fr[r + val2][c + val2]))) / 159.0; } // Gaussian pyramid operation static void getGaussPyramidal(const vpImage &I, vpImage &GI); @@ -966,7 +976,8 @@ class VISP_EXPORT vpImageFilter template static void getGaussianKernel(FilterType *filter, unsigned int size, FilterType sigma = 0., bool normalize = true) { - if ((size % 2) != 1) { + const unsigned int mod2 = 2; + if ((size % mod2) != 1) { throw(vpImageException(vpImageException::incorrectInitializationError, "Bad Gaussian filter size")); } @@ -984,8 +995,9 @@ class VISP_EXPORT vpImageFilter if (normalize) { // renormalization FilterType sum = 0; + const unsigned int val2 = 2U; for (int i = 1; i <= middle; ++i) { - sum += 2 * filter[i]; + sum += val2 * filter[i]; } sum += filter[0]; @@ -1012,7 +1024,8 @@ class VISP_EXPORT vpImageFilter template static void getGaussianDerivativeKernel(FilterType *filter, unsigned int size, FilterType sigma = 0., bool normalize = true) { - if ((size % 2) != 1) { + const unsigned int mod2 = 2; + if ((size % mod2) != 1) { throw(vpImageException(vpImageException::incorrectInitializationError, "Bad Gaussian filter size")); } @@ -1020,7 +1033,8 @@ class VISP_EXPORT vpImageFilter sigma = static_cast((size - 1) / 6.0); } - int middle = (static_cast(size) - 1) / 2; + const int half = 2; + int middle = (static_cast(size) - 1) / half; FilterType sigma2 = static_cast(vpMath::sqr(sigma)); FilterType coef_1 = static_cast(1. / (sigma * sqrt(2. * M_PI))); FilterType coef_1_over_2 = coef_1 / static_cast(2.); @@ -1243,14 +1257,15 @@ class VISP_EXPORT vpImageFilter template inline static FilterType getScharrKernelX(FilterType *filter, unsigned int size) { + const unsigned int actualKernelSize = (size * 2) + 1; if (size != 1) { // Size = 1 => kernel_size = 2*1 + 1 = 3 std::stringstream errMsg; - errMsg << "Cannot get Scharr kernel of size " << ((size * 2) + 1) << " != 3"; + errMsg << "Cannot get Scharr kernel of size " << actualKernelSize << " != 3"; throw vpException(vpException::dimensionError, errMsg.str()); } - vpArray2D ScharrY((size * 2) + 1, (size * 2) + 1); + vpArray2D ScharrY(actualKernelSize, actualKernelSize); FilterType norm = getScharrKernelY(ScharrY.data, size); memcpy(filter, ScharrY.t().data, ScharrY.getRows() * ScharrY.getCols() * sizeof(FilterType)); return norm; @@ -1277,7 +1292,8 @@ class VISP_EXPORT vpImageFilter } const unsigned int kernel_size = (size * 2) + 1; - if (kernel_size == 3) { + const unsigned int kernel3 = 3; + if (kernel_size == kernel3) { memcpy(filter, ScharrY3x3, kernel_size * kernel_size * sizeof(FilterType)); return static_cast(1.0 / 32.0); } @@ -1295,14 +1311,16 @@ class VISP_EXPORT vpImageFilter template inline static FilterType getSobelKernelX(FilterType *filter, unsigned int size) { + const unsigned int maxSize = 20; if (size == 0) { throw vpException(vpException::dimensionError, "Cannot get Sobel kernel of size 0!"); } - if (size > 20) { + if (size > maxSize) { throw vpException(vpException::dimensionError, "Cannot get Sobel kernel of size > 20!"); } - vpArray2D SobelY((size * 2) + 1, (size * 2) + 1); + const unsigned int kernel_size = (size * 2) + 1; + vpArray2D SobelY(kernel_size, kernel_size); FilterType norm = getSobelKernelY(SobelY.data, size); memcpy(filter, SobelY.t().data, SobelY.getRows() * SobelY.getCols() * sizeof(FilterType)); return norm; @@ -1339,26 +1357,28 @@ class VISP_EXPORT vpImageFilter smoothingKernel[index_2][index_1] = 2.0; smoothingKernel[index_2][index_2] = 1.0; + const unsigned int maxSize = 20; if (size == 0) { throw vpException(vpException::dimensionError, "Cannot get Sobel kernel of size 0!"); } - if (size > 20) { + if (size > maxSize) { throw vpException(vpException::dimensionError, "Cannot get Sobel kernel of size > 20!"); } const unsigned int kernel_size = (size * 2) + 1; FilterType scale = static_cast(1. / 8.); // Scale to normalize Sobel3x3 - if (kernel_size == 3) { + const unsigned int kernel3 = 3, kernel5 = 5, kernel7 = 7; + if (kernel_size == kernel3) { memcpy(filter, SobelY3x3, kernel_size * kernel_size * sizeof(FilterType)); return scale; } scale *= static_cast(1. / 16.); // Sobel5x5 is the convolution of smoothingKernel, which needs 1/16 scale factor, with Sobel3x3 - if (kernel_size == 5) { + if (kernel_size == kernel5) { memcpy(filter, SobelY5x5, kernel_size * kernel_size * sizeof(FilterType)); return scale; } scale *= static_cast(1. / 16.); // Sobel7x7 is the convolution of smoothingKernel, which needs 1/16 scale factor, with Sobel5x5 - if (kernel_size == 7) { + if (kernel_size == kernel7) { memcpy(filter, SobelY7x7, kernel_size * kernel_size * sizeof(FilterType)); return scale; } diff --git a/modules/core/include/visp3/core/vpImageTools.h b/modules/core/include/visp3/core/vpImageTools.h index cbcb9bfc3b..f7961ef99f 100644 --- a/modules/core/include/visp3/core/vpImageTools.h +++ b/modules/core/include/visp3/core/vpImageTools.h @@ -510,8 +510,9 @@ inline void vpImageTools::binarise(vpImage &I, unsigned char thre { if (useLUT) { // Construct the LUT - unsigned char lut[256]; - for (unsigned int i = 0; i < 256; ++i) { + const unsigned int sizeLut = 256; + unsigned char lut[sizeLut]; + for (unsigned int i = 0; i < sizeLut; ++i) { lut[i] = i < threshold1 ? value1 : (i > threshold2 ? value3 : value2); } @@ -876,7 +877,8 @@ template void vpImageTools::flip(vpImage &I) vpImage Ibuf; Ibuf.resize(1, width); - for (unsigned int i = 0; i < (height / 2); ++i) { + const unsigned int halfHeight = height / 2; + for (unsigned int i = 0; i < halfHeight; ++i) { memcpy(Ibuf.bitmap, I.bitmap + (i * width), width * sizeof(Type)); memcpy(I.bitmap + (i * width), I.bitmap + ((height - 1 - i) * width), width * sizeof(Type)); @@ -960,7 +962,8 @@ inline void vpImageTools::resizeBicubic(const vpImage &I, vpImage(reinterpret_cast(&p00)[c]), static_cast(reinterpret_cast(&p01)[c]), static_cast(reinterpret_cast(&p02)[c]), @@ -1022,7 +1025,8 @@ inline void vpImageTools::resizeBilinear(const vpImage &I, vpImage(reinterpret_cast(&I[v0][u0])[c]), static_cast(reinterpret_cast(&I[v1][u1])[c]), xFrac); float col1 = lerp(static_cast(reinterpret_cast(&I[v2][u2])[c]), @@ -1092,7 +1096,8 @@ void vpImageTools::resize(const vpImage &I, vpImage &Ires, const vpI #endif ) { - if ((I.getWidth() < 2) || (I.getHeight() < 2) || (Ires.getWidth() < 2) || (Ires.getHeight() < 2)) { + const unsigned int minWidth = 2, minHeight = 2; + if ((I.getWidth() < minWidth) || (I.getHeight() < minHeight) || (Ires.getWidth() < minWidth) || (Ires.getHeight() < minHeight)) { std::cerr << "Input or output image is too small!" << std::endl; return; } @@ -1146,7 +1151,9 @@ inline void vpImageTools::resize(const vpImage &I, vpImage &I, vpImage &Ires #endif ) { - if ((I.getWidth() < 2) || (I.getHeight() < 2) || (Ires.getWidth() < 2) || (Ires.getHeight() < 2)) { + const unsigned int minWidth = 2, minHeight = 2; + + if ((I.getWidth() < minWidth) || (I.getHeight() < minHeight) || (Ires.getWidth() < minWidth) || (Ires.getHeight() < minHeight)) { std::cerr << "Input or output image is too small!" << std::endl; return; } @@ -1220,7 +1229,7 @@ inline void vpImageTools::resize(const vpImage &I, vpImage &Ires #pragma omp parallel for schedule(dynamic) #endif for (int i = 0; i < ires_height; ++i) { - float v = (i + half) * scaleY - half; + float v = ((i + half) * scaleY) - half; float yFrac = v - static_cast(v); unsigned int ires_width = static_cast(Ires.getWidth()); diff --git a/modules/core/include/visp3/core/vpImageTools_warp.h b/modules/core/include/visp3/core/vpImageTools_warp.h index e5d02a8655..7c4d0f14d6 100644 --- a/modules/core/include/visp3/core/vpImageTools_warp.h +++ b/modules/core/include/visp3/core/vpImageTools_warp.h @@ -55,7 +55,8 @@ template void vpImageTools::warpImage(const vpImage &src, const vpMatrix &T, vpImage &dst, const vpImageInterpolationType &interpolation, bool fixedPointArithmetic, bool pixelCenter) { - if (((T.getRows() != 2) && (T.getRows() != 3)) || (T.getCols() != 3)) { + const unsigned int expectedNbCols = 3, expectedNbRows1stOpt = 2, expectedNbRows2ndOpt = 3; + if (((T.getRows() != expectedNbRows1stOpt) && (T.getRows() != expectedNbRows2ndOpt)) || (T.getCols() != expectedNbCols)) { std::cerr << "Input transformation must be a (2x3) or (3x3) matrix." << std::endl; return; } @@ -474,6 +475,7 @@ inline void vpImageTools::warpLinearFixedPointNotCenter(const vpImage &s unsigned int dst_width = dst.getWidth(); int src_height = static_cast(src.getHeight()); int src_width = static_cast(src.getWidth()); + const unsigned char aChannelVal = 255; for (unsigned int i = 0; i < dst_height; ++i) { int64_t xi = a2_i64; int64_t yi = a5_i64; @@ -509,12 +511,12 @@ inline void vpImageTools::warpLinearFixedPointNotCenter(const vpImage &s const float interpB = (interpB_i64 >> (nbits * 2)) + ((interpB_i64 & 0xFFFFFFFFU) * precision_2); dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), - vpMath::saturate(interpB), 255); + vpMath::saturate(interpB), aChannelVal); } else if (y_ < (src_height - 1)) { const vpRGBa val00 = src[y_][x_]; const vpRGBa val10 = src[y_ + 1][x_]; - const int64_t interpR_i64 = static_cast(t_1 * val00.R + t * val10.R); + const int64_t interpR_i64 = static_cast((t_1 * val00.R) + (t * val10.R)); const float interpR = (interpR_i64 >> nbits) + ((interpR_i64 & 0xFFFF) * precision_1); const int64_t interpG_i64 = static_cast((t_1 * val00.G) + (t * val10.G)); @@ -524,7 +526,7 @@ inline void vpImageTools::warpLinearFixedPointNotCenter(const vpImage &s const float interpB = (interpB_i64 >> nbits) + ((interpB_i64 & 0xFFFF) * precision_1); dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), - vpMath::saturate(interpB), 255); + vpMath::saturate(interpB), aChannelVal); } else if (x_ < (src_width - 1)) { const vpRGBa val00 = src[y_][x_]; @@ -539,7 +541,7 @@ inline void vpImageTools::warpLinearFixedPointNotCenter(const vpImage &s const float interpB = (interpB_i64 >> nbits) + ((interpB_i64 & 0xFFFF) * precision_1); dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), - vpMath::saturate(interpB), 255); + vpMath::saturate(interpB), aChannelVal); } else { dst[i][j] = src[y_][x_]; @@ -559,6 +561,7 @@ inline void vpImageTools::warpLinearFixedPointNotCenter(const vpImage &s unsigned int dst_width = dst.getWidth(); int src_height = static_cast(src.getHeight()); int src_width = static_cast(src.getWidth()); + const unsigned char aChannelVal = 255; for (unsigned int i = 0; i < dst_height; ++i) { int64_t xi = a2_i64; int64_t yi = a5_i64; @@ -595,7 +598,7 @@ inline void vpImageTools::warpLinearFixedPointNotCenter(const vpImage &s const float interpB = lerp(colB0, colB1, t); dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), - vpMath::saturate(interpB), 255); + vpMath::saturate(interpB), aChannelVal); } else if (y_ < (src_height - 1)) { const vpRGBa val00 = src[y_][x_]; @@ -605,7 +608,7 @@ inline void vpImageTools::warpLinearFixedPointNotCenter(const vpImage &s const float interpB = lerp(val00.B, val10.B, t); dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), - vpMath::saturate(interpB), 255); + vpMath::saturate(interpB), aChannelVal); } else if (x_ < (src_width - 1)) { const vpRGBa val00 = src[y_][x_]; @@ -615,7 +618,7 @@ inline void vpImageTools::warpLinearFixedPointNotCenter(const vpImage &s const float interpB = lerp(val00.B, val01.B, s); dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), - vpMath::saturate(interpB), 255); + vpMath::saturate(interpB), aChannelVal); } else { dst[i][j] = src[y_][x_]; @@ -658,6 +661,7 @@ inline void vpImageTools::warpLinear(const vpImage &src, const vpMatrix unsigned int dst_width = dst.getWidth(); int src_height = static_cast(src.getHeight()); int src_width = static_cast(src.getWidth()); + const unsigned char aChannelVal = 255; for (unsigned int i = 0; i < dst_height; ++i) { for (unsigned int j = 0; j < dst_width; ++j) { double x = (a0 * (centerCorner ? (j + 0.5) : j)) + (a1 * (centerCorner ? (i + 0.5) : i)) + a2; @@ -696,7 +700,7 @@ inline void vpImageTools::warpLinear(const vpImage &src, const vpMatrix const double interpB = lerp(colB0, colB1, t); dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), - vpMath::saturate(interpB), 255); + vpMath::saturate(interpB), aChannelVal); } else if (y_lower < (src_height - 1)) { const vpRGBa val00 = src[y_lower][x_lower]; @@ -706,7 +710,7 @@ inline void vpImageTools::warpLinear(const vpImage &src, const vpMatrix const double interpB = lerp(val00.B, val10.B, t); dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), - vpMath::saturate(interpB), 255); + vpMath::saturate(interpB), aChannelVal); } else if (x_lower < (src_width - 1)) { const vpRGBa val00 = src[y_lower][x_lower]; @@ -716,7 +720,7 @@ inline void vpImageTools::warpLinear(const vpImage &src, const vpMatrix const double interpB = lerp(val00.B, val01.B, s); dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), - vpMath::saturate(interpB), 255); + vpMath::saturate(interpB), aChannelVal); } else { dst[i][j] = src[y_lower][x_lower]; diff --git a/modules/imgproc/include/visp3/imgproc/vpCircleHoughTransform.h b/modules/imgproc/include/visp3/imgproc/vpCircleHoughTransform.h index 17e10b61be..676b68125d 100644 --- a/modules/imgproc/include/visp3/imgproc/vpCircleHoughTransform.h +++ b/modules/imgproc/include/visp3/imgproc/vpCircleHoughTransform.h @@ -1195,6 +1195,19 @@ class VISP_EXPORT vpCircleHoughTransform static const unsigned char edgeMapOff; protected: + +#ifndef DOXYGEN_SHOULD_SKIP_THIS + /** + * \brief Data storage for the computation of the barycenter of center candidates. + */ + typedef struct vpCentersBarycenter + { + std::pair m_position; /*!< The position of the barycenter.*/ + float m_totalVotes; /*!< The cumulated number of votes accross all the centers voting for this barycenter.*/ + float m_nbElectors; /*!< The number of center candidates that have voted for this barycenter.*/ + }vpCentersBarycenter; +#endif + /** * \brief Initialize the Gaussian filters used to blur the image and * compute the gradient images. @@ -1241,6 +1254,19 @@ class VISP_EXPORT vpCircleHoughTransform */ virtual void filterCenterCandidates(const std::vector &peak_positions_votes); + /** + * \brief Look in the list containing the raw center candidates if one is closed to the center candidate + * that is currently of interest. + * + * \param[in] idPeak The ID of the center candidate that is currently of interest. + * \param[in] nbPeaks The number of center candidates. + * \param[in] squared_distance_max The maximum squared distance between to center candidates to merge them. + * \param[in] peak_positions_votes The list containing the raw center candidates. + * \param[out] has_been_merged Vector indicating if the center candidates have already been merged. + * \return vpCentersBarycenter The barycenter between the center candidate that is currently of interest and similar center candidates. + */ + virtual vpCentersBarycenter mergeSimilarCenters(const unsigned int &idPeak, const unsigned int &nbPeaks, const float &squared_distance_max, const std::vector &peak_positions_votes, std::vector &has_been_merged); + /** * \brief Compute the probability of \b circle given the number of pixels voting for * it \b nbVotes. diff --git a/modules/imgproc/include/visp3/imgproc/vpContours.h b/modules/imgproc/include/visp3/imgproc/vpContours.h index 6c59a0dce8..fd11a5abea 100644 --- a/modules/imgproc/include/visp3/imgproc/vpContours.h +++ b/modules/imgproc/include/visp3/imgproc/vpContours.h @@ -144,7 +144,7 @@ class vpDirection { vpDirection direction; int directionSize = LAST_DIRECTION; - direction.m_direction = vpDirectionType((static_cast(m_direction) + 1) % directionSize); + direction.m_direction = static_cast((static_cast(m_direction) + 1) % directionSize); return direction; } @@ -158,7 +158,7 @@ class vpDirection vpDirection direction; int directionSize = static_cast(LAST_DIRECTION); int idx = VISP_NAMESPACE_ADDRESSING vpMath::modulo(static_cast(m_direction) - 1, directionSize); - direction.m_direction = vpDirectionType(idx); + direction.m_direction = static_cast(idx); return direction; } diff --git a/modules/imgproc/src/vpCircleHoughTransform.cpp b/modules/imgproc/src/vpCircleHoughTransform.cpp deleted file mode 100644 index a37cfda7b8..0000000000 --- a/modules/imgproc/src/vpCircleHoughTransform.cpp +++ /dev/null @@ -1,1210 +0,0 @@ -/* - * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2024 by Inria. All rights reserved. - * - * This software is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * See the file LICENSE.txt at the root directory of this source - * distribution for additional information about the GNU GPL. - * - * For using ViSP with software that can not be combined with the GNU - * GPL, please contact Inria about acquiring a ViSP Professional - * Edition License. - * - * See https://visp.inria.fr for more information. - * - * This software was developed at: - * Inria Rennes - Bretagne Atlantique - * Campus Universitaire de Beaulieu - * 35042 Rennes Cedex - * France - * - * If you have questions regarding the use of this file, please contact - * Inria at visp@inria.fr - * - * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE - * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - */ - -#include -#include - -#include - -BEGIN_VISP_NAMESPACE - -#if (VISP_CXX_STANDARD == VISP_CXX_STANDARD_98) -namespace -{ - -// Sorting by decreasing probabilities -bool hasBetterProba(std::pair a, std::pair b) -{ - return (a.second > b.second); -} - -void updateAccumulator(const float &x_orig, const float &y_orig, - const int &x, const int &y, - const int &offsetX, const int &offsetY, - const int &nbCols, const int &nbRows, - vpImage &accum, bool &hasToStop) -{ - if (((x - offsetX) < 0) || - ((x - offsetX) >= nbCols) || - ((y - offsetY) < 0) || - ((y - offsetY) >= nbRows) - ) { - hasToStop = true; - } - else { - float dx = (x_orig - static_cast(x)); - float dy = (y_orig - static_cast(y)); - accum[y - offsetY][x - offsetX] += std::abs(dx) + std::abs(dy); - } -} - -bool sortingCenters(const vpCircleHoughTransform::vpCenterVotes &position_vote_a, - const vpCircleHoughTransform::vpCenterVotes &position_vote_b) -{ - return position_vote_a.m_votes > position_vote_b.m_votes; -} - -float computeEffectiveRadius(const float &votes, const float &weigthedSumRadius) -{ - float r_effective = -1.f; - if (votes > std::numeric_limits::epsilon()) { - r_effective = weigthedSumRadius / votes; - } - return r_effective; -} - -void -scaleFilter(vpArray2D &filter, const float &scale) -{ - unsigned int nbRows = filter.getRows(); - unsigned int nbCols = filter.getCols(); - for (unsigned int r = 0; r < nbRows; ++r) { - for (unsigned int c = 0; c < nbCols; ++c) { - filter[r][c] = filter[r][c] * scale; - } - } -} -} -#endif - -// Static variables -const unsigned char vpCircleHoughTransform::edgeMapOn = 255; -const unsigned char vpCircleHoughTransform::edgeMapOff = 0; - -vpCircleHoughTransform::vpCircleHoughTransform() - : m_algoParams() - , mp_mask(nullptr) -{ - initGaussianFilters(); - initGradientFilters(); -} - -vpCircleHoughTransform::vpCircleHoughTransform(const vpCircleHoughTransformParams &algoParams) - : m_algoParams(algoParams) - , mp_mask(nullptr) -{ - initGaussianFilters(); - initGradientFilters(); -} - -void -vpCircleHoughTransform::init(const vpCircleHoughTransformParams &algoParams) -{ - m_algoParams = algoParams; - initGaussianFilters(); - initGradientFilters(); -} - -vpCircleHoughTransform::~vpCircleHoughTransform() -{ } - -#ifdef VISP_HAVE_NLOHMANN_JSON -using json = nlohmann::json; - -vpCircleHoughTransform::vpCircleHoughTransform(const std::string &jsonPath) -{ - initFromJSON(jsonPath); -} - -void -vpCircleHoughTransform::initFromJSON(const std::string &jsonPath) -{ - std::ifstream file(jsonPath); - if (!file.good()) { - std::stringstream ss; - ss << "Problem opening file " << jsonPath << ". Make sure it exists and is readable" << std::endl; - throw vpException(vpException::ioError, ss.str()); - } - json j; - try { - j = json::parse(file); - } - catch (json::parse_error &e) { - std::stringstream msg; - msg << "Could not parse JSON file : \n"; - - msg << e.what() << std::endl; - msg << "Byte position of error: " << e.byte; - throw vpException(vpException::ioError, msg.str()); - } - m_algoParams = j; // Call from_json(const json& j, vpDetectorDNN& *this) to read json - file.close(); - initGaussianFilters(); - initGradientFilters(); -} - -void -vpCircleHoughTransform::saveConfigurationInJSON(const std::string &jsonPath) const -{ - m_algoParams.saveConfigurationInJSON(jsonPath); -} -#endif - -void -vpCircleHoughTransform::initGaussianFilters() -{ - const int filterHalfSize = (m_algoParams.m_gaussianKernelSize + 1) / 2; - m_fg.resize(1, filterHalfSize); - vpImageFilter::getGaussianKernel(m_fg.data, m_algoParams.m_gaussianKernelSize, m_algoParams.m_gaussianStdev, true); - m_cannyVisp.setGaussianFilterParameters(m_algoParams.m_gaussianKernelSize, m_algoParams.m_gaussianStdev); -} - -void -vpCircleHoughTransform::initGradientFilters() -{ -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) // Check if cxx11 or higher - // Helper to apply the scale to the raw values of the filters - auto scaleFilter = [](vpArray2D &filter, const float &scale) { - const unsigned int nbRows = filter.getRows(); - const unsigned int nbCols = filter.getCols(); - for (unsigned int r = 0; r < nbRows; ++r) { - for (unsigned int c = 0; c < nbCols; ++c) { - filter[r][c] = filter[r][c] * scale; - } - } - }; -#endif - - const int moduloCheckForOddity = 2; - if ((m_algoParams.m_gradientFilterKernelSize % moduloCheckForOddity) != 1) { - throw vpException(vpException::badValue, "Gradient filters Kernel size should be odd."); - } - m_gradientFilterX.resize(m_algoParams.m_gradientFilterKernelSize, m_algoParams.m_gradientFilterKernelSize); - m_gradientFilterY.resize(m_algoParams.m_gradientFilterKernelSize, m_algoParams.m_gradientFilterKernelSize); - m_cannyVisp.setGradientFilterAperture(m_algoParams.m_gradientFilterKernelSize); - - float scaleX = 1.f; - float scaleY = 1.f; - unsigned int filterHalfSize = (m_algoParams.m_gradientFilterKernelSize - 1) / 2; - - if (m_algoParams.m_filteringAndGradientType == vpImageFilter::CANNY_GBLUR_SOBEL_FILTERING) { - // Compute the Sobel filters - scaleX = vpImageFilter::getSobelKernelX(m_gradientFilterX.data, filterHalfSize); - scaleY = vpImageFilter::getSobelKernelY(m_gradientFilterY.data, filterHalfSize); - } - else if (m_algoParams.m_filteringAndGradientType == vpImageFilter::CANNY_GBLUR_SCHARR_FILTERING) { - // Compute the Scharr filters - scaleX = vpImageFilter::getScharrKernelX(m_gradientFilterX.data, filterHalfSize); - scaleY = vpImageFilter::getScharrKernelY(m_gradientFilterY.data, filterHalfSize); - } - else { - std::string errMsg = "[vpCircleHoughTransform::initGradientFilters] Error: gradient filtering method \""; - errMsg += vpImageFilter::vpCannyFiltAndGradTypeToStr(m_algoParams.m_filteringAndGradientType); - errMsg += "\" has not been implemented yet\n"; - throw vpException(vpException::notImplementedError, errMsg); - } - scaleFilter(m_gradientFilterX, scaleX); - scaleFilter(m_gradientFilterY, scaleY); -} - -std::vector -vpCircleHoughTransform::detect(const vpImage &I) -{ - vpImage I_gray; - vpImageConvert::convert(I, I_gray); - return detect(I_gray); -} - -#ifdef HAVE_OPENCV_CORE -std::vector -vpCircleHoughTransform::detect(const cv::Mat &cv_I) -{ - vpImage I_gray; - vpImageConvert::convert(cv_I, I_gray); - return detect(I_gray); -} -#endif - -std::vector -vpCircleHoughTransform::detect(const vpImage &I, const int &nbCircles) -{ - std::vector detections = detect(I); - size_t nbDetections = detections.size(); - - // Prepare vector of tuple to sort by decreasing probabilities - std::vector > v_id_proba; - for (size_t i = 0; i < nbDetections; ++i) { - std::pair id_proba(i, m_finalCirclesProbabilities[i]); - v_id_proba.push_back(id_proba); - } - -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) - // Sorting by decreasing probabilities - auto hasBetterProba - = [](std::pair a, std::pair b) { - return (a.second > b.second); - }; -#endif - std::sort(v_id_proba.begin(), v_id_proba.end(), hasBetterProba); - - // Clearing the storages containing the detection results - // to have it sorted by decreasing probabilities - size_t limitMin; - if (nbCircles < 0) { - limitMin = nbDetections; - } - else { - limitMin = std::min(nbDetections, static_cast(nbCircles)); - } - - std::vector bestCircles; - std::vector copyFinalCircles = m_finalCircles; - std::vector copyFinalCirclesVotes = m_finalCircleVotes; - std::vector copyFinalCirclesProbas = m_finalCirclesProbabilities; - std::vector > > copyFinalCirclesVotingPoints = m_finalCirclesVotingPoints; - for (size_t i = 0; i < nbDetections; ++i) { - size_t id = v_id_proba[i].first; - m_finalCircles[i] = copyFinalCircles[id]; - m_finalCircleVotes[i] = copyFinalCirclesVotes[id]; - m_finalCirclesProbabilities[i] = copyFinalCirclesProbas[id]; - if (m_algoParams.m_recordVotingPoints) { - m_finalCirclesVotingPoints[i] = copyFinalCirclesVotingPoints[id]; - } - if (i < limitMin) { - bestCircles.push_back(m_finalCircles[i]); - } - } - - return bestCircles; -} - -std::vector -vpCircleHoughTransform::detect(const vpImage &I) -{ - // Cleaning results of potential previous detection - m_centerCandidatesList.clear(); - m_centerVotes.clear(); - m_edgePointsList.clear(); - m_circleCandidates.clear(); - m_circleCandidatesVotes.clear(); - m_circleCandidatesProbabilities.clear(); - m_finalCircles.clear(); - m_finalCircleVotes.clear(); - - // Ensuring that the difference between the max and min radii is big enough to take into account - // the pixelization of the image - const float minRadiusDiff = 3.f; - if ((m_algoParams.m_maxRadius - m_algoParams.m_minRadius) < minRadiusDiff) { - if (m_algoParams.m_minRadius > (minRadiusDiff / 2.f)) { - m_algoParams.m_maxRadius += minRadiusDiff / 2.f; - m_algoParams.m_minRadius -= minRadiusDiff / 2.f; - } - else { - m_algoParams.m_maxRadius += minRadiusDiff - m_algoParams.m_minRadius; - m_algoParams.m_minRadius = 0.f; - } - } - - // Ensuring that the difference between the max and min center position is big enough to take into account - // the pixelization of the image - const float minCenterPositionDiff = 3.f; - if ((m_algoParams.m_centerXlimits.second - m_algoParams.m_centerXlimits.first) < minCenterPositionDiff) { - m_algoParams.m_centerXlimits.second += static_cast(minCenterPositionDiff / 2.f); - m_algoParams.m_centerXlimits.first -= static_cast(minCenterPositionDiff / 2.f); - } - if ((m_algoParams.m_centerYlimits.second - m_algoParams.m_centerYlimits.first) < minCenterPositionDiff) { - m_algoParams.m_centerYlimits.second += static_cast(minCenterPositionDiff / 2.f); - m_algoParams.m_centerYlimits.first -= static_cast(minCenterPositionDiff / 2.f); - } - - // First thing, we need to apply a Gaussian filter on the image to remove some spurious noise - // Then, we need to compute the image gradients in order to be able to perform edge detection - computeGradients(I); - - // Using the gradients, it is now possible to perform edge detection - // We rely on the Canny edge detector - // It will also give us the connected edged points - edgeDetection(I); - - // From the edge map and gradient information, it is possible to compute - // the center point candidates - computeCenterCandidates(); - - // From the edge map and center point candidates, we can compute candidate - // circles. These candidate circles are circles whose center belong to - // the center point candidates and whose radius is a "radius bin" that got - // enough votes by computing the distance between each point of the edge map - // and the center point candidate - computeCircleCandidates(); - - // Finally, we perform a merging operation that permits to merge circles - // respecting similarity criteria (distance between centers and similar radius) - mergeCircleCandidates(); - - return m_finalCircles; -} - -bool -operator==(const vpImageCircle &a, const vpImageCircle &b) -{ - vpImagePoint aCenter = a.getCenter(); - vpImagePoint bCenter = b.getCenter(); - bool haveSameCenter = (std::abs(aCenter.get_u() - bCenter.get_u()) - + std::abs(aCenter.get_v() - bCenter.get_v())) <= (2. * std::numeric_limits::epsilon()); - bool haveSameRadius = std::abs(a.getRadius() - b.getRadius()) <= (2.f * std::numeric_limits::epsilon()); - return (haveSameCenter && haveSameRadius); -} - -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) -void vpCircleHoughTransform::computeVotingMask(const vpImage &I, const std::vector &detections, - std::optional< vpImage > &mask, std::optional>>> &opt_votingPoints) const -#else -void vpCircleHoughTransform::computeVotingMask(const vpImage &I, const std::vector &detections, - vpImage **mask, std::vector > > **opt_votingPoints) const -#endif -{ - if (!m_algoParams.m_recordVotingPoints) { - // We weren't asked to remember the voting points -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) - mask = std::nullopt; - opt_votingPoints = std::nullopt; -#else - *mask = nullptr; - *opt_votingPoints = nullptr; -#endif - return; - } - -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) - mask = vpImage(I.getHeight(), I.getWidth(), false); - opt_votingPoints = std::vector>>(); -#else - *mask = new vpImage(I.getHeight(), I.getWidth(), false); - *opt_votingPoints = new std::vector > >(); -#endif - -#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98) - for (const auto &detection : detections) -#else - const size_t nbDetections = detections.size(); - for (size_t i = 0; i < nbDetections; ++i) -#endif - { - bool hasFoundSimilarCircle = false; - unsigned int nbPreviouslyDetected = static_cast(m_finalCircles.size()); - unsigned int id = 0; - // Looking for a circle that was detected and is similar to the one given to the function - while ((id < nbPreviouslyDetected) && (!hasFoundSimilarCircle)) { - vpImageCircle previouslyDetected = m_finalCircles[id]; -#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98) - if (previouslyDetected == detection) -#else - if (previouslyDetected == detections[i]) -#endif - { - hasFoundSimilarCircle = true; - // We found a circle that is similar to the one given to the function => updating the mask - const unsigned int nbVotingPoints = static_cast(m_finalCirclesVotingPoints[id].size()); - for (unsigned int idPoint = 0; idPoint < nbVotingPoints; ++idPoint) { - const std::pair &votingPoint = m_finalCirclesVotingPoints[id][idPoint]; -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) - (*mask)[votingPoint.first][votingPoint.second] = true; -#else - (**mask)[votingPoint.first][votingPoint.second] = true; -#endif - } -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) - opt_votingPoints->push_back(m_finalCirclesVotingPoints[id]); -#else - (**opt_votingPoints).push_back(m_finalCirclesVotingPoints[id]); -#endif - } - ++id; - } - } -} - -void -vpCircleHoughTransform::computeGradients(const vpImage &I) -{ - if ((m_algoParams.m_filteringAndGradientType == vpImageFilter::CANNY_GBLUR_SOBEL_FILTERING) - || (m_algoParams.m_filteringAndGradientType == vpImageFilter::CANNY_GBLUR_SCHARR_FILTERING)) { - // Computing the Gaussian blurr - vpImage Iblur, GIx; - vpImageFilter::filterX(I, GIx, m_fg.data, m_algoParams.m_gaussianKernelSize, mp_mask); - vpImageFilter::filterY(GIx, Iblur, m_fg.data, m_algoParams.m_gaussianKernelSize, mp_mask); - - // Computing the gradients - vpImageFilter::filter(Iblur, m_dIx, m_gradientFilterX, true, mp_mask); - vpImageFilter::filter(Iblur, m_dIy, m_gradientFilterY, true, mp_mask); - } - else { - std::string errMsg("[computeGradients] The filtering + gradient operators \""); - errMsg += vpImageFilter::vpCannyFiltAndGradTypeToStr(m_algoParams.m_filteringAndGradientType); - errMsg += "\" is not implemented (yet)."; - throw(vpException(vpException::notImplementedError, errMsg)); - } -} - -void -vpCircleHoughTransform::edgeDetection(const vpImage &I) -{ - if (m_algoParams.m_cannyBackendType == vpImageFilter::CANNY_VISP_BACKEND) { - // This is done to increase the time performances, because it avoids to - // recompute the gradient in the vpImageFilter::canny method - m_cannyVisp.setFilteringAndGradientType(m_algoParams.m_filteringAndGradientType); - m_cannyVisp.setCannyThresholds(m_algoParams.m_lowerCannyThresh, m_algoParams.m_upperCannyThresh); - m_cannyVisp.setCannyThresholdsRatio(m_algoParams.m_lowerCannyThreshRatio, m_algoParams.m_upperCannyThreshRatio); - m_cannyVisp.setGradients(m_dIx, m_dIy); - m_cannyVisp.setMask(mp_mask); - m_edgeMap = m_cannyVisp.detect(I); - } - else { - if (mp_mask != nullptr) { - // Delete pixels that fall outside the mask - vpImage I_masked(I); - unsigned int nbRows = I_masked.getHeight(); - unsigned int nbCols = I_masked.getWidth(); - for (unsigned int r = 0; r < nbRows; ++r) { - for (unsigned int c = 0; c < nbCols; ++c) { - if (!((*mp_mask)[r][c])) { - I_masked[r][c] = 0; - } - } - } - - // We will have to recompute the gradient in the desired backend format anyway so we let - // the vpImageFilter::canny method take care of it - vpImageFilter::canny(I_masked, m_edgeMap, m_algoParams.m_gaussianKernelSize, m_algoParams.m_lowerCannyThresh, - m_algoParams.m_upperCannyThresh, m_algoParams.m_gradientFilterKernelSize, m_algoParams.m_gaussianStdev, - m_algoParams.m_lowerCannyThreshRatio, m_algoParams.m_upperCannyThreshRatio, true, - m_algoParams.m_cannyBackendType, m_algoParams.m_filteringAndGradientType); - } - else { - vpImageFilter::canny(I, m_edgeMap, m_algoParams.m_gaussianKernelSize, m_algoParams.m_lowerCannyThresh, - m_algoParams.m_upperCannyThresh, m_algoParams.m_gradientFilterKernelSize, m_algoParams.m_gaussianStdev, - m_algoParams.m_lowerCannyThreshRatio, m_algoParams.m_upperCannyThreshRatio, true, - m_algoParams.m_cannyBackendType, m_algoParams.m_filteringAndGradientType); - } - } - - for (int i = 0; i < m_algoParams.m_edgeMapFilteringNbIter; ++i) { - filterEdgeMap(); - } -} - -void -vpCircleHoughTransform::filterEdgeMap() -{ - vpImage J = m_edgeMap; - const unsigned int height = J.getHeight(); - const unsigned int width = J.getWidth(); - const int minNbContiguousPts = 2; - - for (unsigned int i = 1; i < (height - 1); ++i) { - for (unsigned int j = 1; j < (width - 1); ++j) { - if (J[i][j] == vpCircleHoughTransform::edgeMapOn) { - // Consider 8 neighbors - int topLeftPixel = static_cast(J[i - 1][j - 1]); - int topPixel = static_cast(J[i - 1][j]); - int topRightPixel = static_cast(J[i - 1][j + 1]); - int botLeftPixel = static_cast(J[i + 1][j - 1]); - int bottomPixel = static_cast(J[i + 1][j]); - int botRightPixel = static_cast(J[i + 1][j + 1]); - int leftPixel = static_cast(J[i][j - 1]); - int rightPixel = static_cast(J[i][j + 1]); - if ((topLeftPixel + topPixel + topRightPixel - + botLeftPixel + bottomPixel + botRightPixel - + leftPixel + rightPixel - ) >= (minNbContiguousPts * static_cast(vpCircleHoughTransform::edgeMapOn))) { - // At least minNbContiguousPts of the 8-neighbor points are also an edge point - // so we keep the edge point - m_edgeMap[i][j] = vpCircleHoughTransform::edgeMapOn; - } - else { - // The edge point is isolated => we erase it - m_edgeMap[i][j] = vpCircleHoughTransform::edgeMapOff; - } - } - } - } -} - -void -vpCircleHoughTransform::computeCenterCandidates() -{ - // For each edge point EP_i, check the image gradient at EP_i - // Then, for each image point in the direction of the gradient, - // increment the accumulator - // We can perform bilinear interpolation in order not to vote for a "line" of - // points, but for an "area" of points - unsigned int nbRows = m_edgeMap.getRows(), nbCols = m_edgeMap.getCols(); - - // Computing the minimum and maximum horizontal position of the center candidates - // 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); - float minimumXpositionFloat = static_cast(minimumXposition); - float maximumXpositionFloat = static_cast(maximumXposition); - int offsetX = minimumXposition; - int accumulatorWidth = (maximumXposition - minimumXposition) + 1; - if (accumulatorWidth <= 0) { - throw(vpException(vpException::dimensionError, "[vpCircleHoughTransform::computeCenterCandidates] Accumulator width <= 0!")); - } - - // Computing the minimum and maximum vertical position of the center candidates - // 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 + nbRows)); - minimumYposition = std::min(minimumYposition, maximumYposition - 1); - float minimumYpositionFloat = static_cast(minimumYposition); - float maximumYpositionFloat = static_cast(maximumYposition); - int offsetY = minimumYposition; - int accumulatorHeight = (maximumYposition - minimumYposition) + 1; - if (accumulatorHeight <= 0) { - throw(vpException(vpException::dimensionError, "[vpCircleHoughTransform::computeCenterCandidates] Accumulator height <= 0!")); - } - - vpImage centersAccum(accumulatorHeight, accumulatorWidth + 1, 0.); /*!< Votes for the center candidates.*/ - - const int nbDirections = 2; - for (unsigned int r = 0; r < nbRows; ++r) { - for (unsigned int c = 0; c < nbCols; ++c) { - if (m_edgeMap[r][c] == vpCircleHoughTransform::edgeMapOn) { - // Voting for points in both direction of the gradient - // Step from min_radius to max_radius in both directions of the gradient - float mag = std::sqrt((m_dIx[r][c] * m_dIx[r][c]) + (m_dIy[r][c] * m_dIy[r][c])); - - float sx = 0.f, sy = 0.f; - if (std::abs(mag) >= std::numeric_limits::epsilon()) { - sx = m_dIx[r][c] / mag; - sy = m_dIy[r][c] / mag; - - // Saving the edge point for further use - m_edgePointsList.push_back(std::pair(r, c)); - - for (int k1 = 0; k1 < nbDirections; ++k1) { - bool hasToStopLoop = false; - int x_low_prev = std::numeric_limits::max(), y_low_prev, y_high_prev; - int x_high_prev = (y_low_prev = (y_high_prev = x_low_prev)); - - float rstart = m_algoParams.m_minRadius, rstop = m_algoParams.m_maxRadius; - float min_minus_c = minimumXpositionFloat - static_cast(c); - float min_minus_r = minimumYpositionFloat - static_cast(r); - float max_minus_c = maximumXpositionFloat - static_cast(c); - 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); - float rmax = max_minus_c / sx; - 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); - float rmax = min_minus_c / sx; - rstop = std::min(rmax, m_algoParams.m_maxRadius); - } - - if (sy > 0) { - float rmin = min_minus_r / sy; - rstart = std::max(rmin, rstart); - float rmax = max_minus_r / sy; - rstop = std::min(rmax, rstop); - } - else if (sy < 0) { - float rmin = max_minus_r / sy; - rstart = std::max(rmin, rstart); - float rmax = min_minus_r / sy; - rstop = std::min(rmax, rstop); - } - - float deltar_x = 1.f / std::abs(sx), deltar_y = 1.f / std::abs(sy); - float deltar = std::min(deltar_x, deltar_y); - - float rad = rstart; - while ((rad <= rstop) && (!hasToStopLoop)) { - float x1 = static_cast(c) + (rad * sx); - float y1 = static_cast(r) + (rad * sy); - rad += deltar; // Update rad that is not used below not to forget it - - if ((x1 < minimumXpositionFloat) || (y1 < minimumYpositionFloat) - || (x1 > maximumXpositionFloat) || (y1 > maximumYpositionFloat)) { - continue; // It means that the center is outside the search region. - } - - int x_low, x_high, y_low, y_high; - - if (x1 > 0.) { - x_low = static_cast(std::floor(x1)); - x_high = static_cast(std::ceil(x1)); - } - else { - x_low = -(static_cast(std::ceil(-x1))); - x_high = -(static_cast(std::floor(-x1))); - } - - if (y1 > 0.) { - y_low = static_cast(std::floor(y1)); - y_high = static_cast(std::ceil(y1)); - } - else { - y_low = -(static_cast(std::ceil(-1. * y1))); - y_high = -(static_cast(std::floor(-1. * y1))); - } - - if ((x_low_prev == x_low) && (x_high_prev == x_high) - && (y_low_prev == y_low) && (y_high_prev == y_high)) { - // Avoid duplicated votes to the same center candidate - continue; - } - else { - x_low_prev = x_low; - x_high_prev = x_high; - y_low_prev = y_low; - y_high_prev = y_high; - } - -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) - auto updateAccumulator = - [](const float &x_orig, const float &y_orig, const int &x, const int &y, - const int &offsetX, const int &offsetY, const int &nbCols, const int &nbRows, - vpImage &accum, bool &hasToStop) { - if (((x - offsetX) < 0) || ((x - offsetX) >= nbCols) || - ((y - offsetY) < 0) || ((y - offsetY) >= nbRows) - ) { - hasToStop = true; - } - else { - float dx = (x_orig - static_cast(x)); - float dy = (y_orig - static_cast(y)); - accum[y - offsetY][x - offsetX] += std::abs(dx) + std::abs(dy); - } - }; -#endif - - updateAccumulator(x1, y1, x_low, y_low, - offsetX, offsetY, - accumulatorWidth, accumulatorHeight, - centersAccum, hasToStopLoop - ); - - updateAccumulator(x1, y1, x_high, y_high, - offsetX, offsetY, - accumulatorWidth, accumulatorHeight, - centersAccum, hasToStopLoop - ); - } - - sx = -sx; - sy = -sy; - } - } - } - } - } - - // 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 - vpImageMorphology::dilatation(centerCandidatesMaxima, dilatationKernelSize); - - // Look for the image points that correspond to the accumulator maxima - // These points will become the center candidates - // find the possible circle centers - int nbColsAccum = centersAccum.getCols(); - int nbRowsAccum = centersAccum.getRows(); - int nbVotes = -1; - std::vector peak_positions_votes; - - for (int y = 0; y < nbRowsAccum; ++y) { - int left = -1; - for (int x = 0; x < nbColsAccum; ++x) { - if ((centersAccum[y][x] >= m_algoParams.m_centerMinThresh) - && (vpMath::equal(centersAccum[y][x], centerCandidatesMaxima[y][x])) - && (centersAccum[y][x] > centersAccum[y][x + 1]) - ) { - if (left < 0) { - left = 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., 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); - for (int r = startingRow; r < endRow; ++r) { - for (int c = startingCol; c < endCol; ++c) { - sumVotes += centersAccum[r][c]; - x_avg += centersAccum[r][c] * c; - y_avg += centersAccum[r][c] * r; - } - } - float avgVotes = sumVotes / static_cast(m_algoParams.m_averagingWindowSize * m_algoParams.m_averagingWindowSize); - if (avgVotes > m_algoParams.m_centerMinThresh) { - x_avg /= static_cast(sumVotes); - y_avg /= static_cast(sumVotes); - std::pair position(y_avg + static_cast(offsetY), x_avg + static_cast(offsetX)); - vpCenterVotes position_vote; - position_vote.m_position = position; - position_vote.m_votes = avgVotes; - peak_positions_votes.push_back(position_vote); - } - if (nbVotes < 0) { - std::stringstream errMsg; - errMsg << "nbVotes (" << nbVotes << ") < 0, thresh = " << m_algoParams.m_centerMinThresh; - throw(vpException(vpException::badValue, errMsg.str())); - } - left = -1; - nbVotes = -1; - } - } - } - filterCenterCandidates(peak_positions_votes); -} - -void -vpCircleHoughTransform::filterCenterCandidates(const std::vector &peak_positions_votes) -{ - unsigned int nbPeaks = static_cast(peak_positions_votes.size()); - if (nbPeaks > 0) { - std::vector has_been_merged(nbPeaks, false); - std::vector merged_peaks_position_votes; - float squared_distance_max = m_algoParams.m_centerMinDist * m_algoParams.m_centerMinDist; - for (unsigned int idPeak = 0; idPeak < nbPeaks; ++idPeak) { - float votes = peak_positions_votes[idPeak].m_votes; - if (has_been_merged[idPeak]) { - // Ignoring peak that has already been merged - continue; - } - else if (votes < m_algoParams.m_centerMinThresh) { - // Ignoring peak whose number of votes is lower than the threshold - has_been_merged[idPeak] = true; - continue; - } - std::pair position = peak_positions_votes[idPeak].m_position; - std::pair barycenter; - barycenter.first = position.first * peak_positions_votes[idPeak].m_votes; - barycenter.second = position.second * peak_positions_votes[idPeak].m_votes; - float total_votes = peak_positions_votes[idPeak].m_votes; - float nb_electors = 1.f; - // Looking for potential similar peak in the following peaks - for (unsigned int idCandidate = idPeak + 1; idCandidate < nbPeaks; ++idCandidate) { - float votes_candidate = peak_positions_votes[idCandidate].m_votes; - if (has_been_merged[idCandidate]) { - continue; - } - else if (votes_candidate < m_algoParams.m_centerMinThresh) { - // Ignoring peak whose number of votes is lower than the threshold - has_been_merged[idCandidate] = true; - continue; - } - // Computing the distance with the peak of insterest - std::pair position_candidate = peak_positions_votes[idCandidate].m_position; - float squared_distance = ((position.first - position_candidate.first) * (position.first - position_candidate.first)) - + ((position.second - position_candidate.second) * (position.second - position_candidate.second)); - - // If the peaks are similar, update the barycenter peak between them and corresponding votes - if (squared_distance < squared_distance_max) { - barycenter.first += position_candidate.first * votes_candidate; - barycenter.second += position_candidate.second * votes_candidate; - total_votes += votes_candidate; - nb_electors += 1.f; - has_been_merged[idCandidate] = true; - } - } - - float avg_votes = total_votes / nb_electors; - // Only the centers having enough votes are considered - if (avg_votes > m_algoParams.m_centerMinThresh) { - barycenter.first /= total_votes; - barycenter.second /= total_votes; - vpCenterVotes barycenter_votes; - barycenter_votes.m_position = barycenter; - barycenter_votes.m_votes = avg_votes; - merged_peaks_position_votes.push_back(barycenter_votes); - } - } - -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) - auto sortingCenters = [](const vpCenterVotes &position_vote_a, - const vpCenterVotes &position_vote_b) { - return position_vote_a.m_votes > position_vote_b.m_votes; - }; -#endif - - std::sort(merged_peaks_position_votes.begin(), merged_peaks_position_votes.end(), sortingCenters); - - nbPeaks = static_cast(merged_peaks_position_votes.size()); - int nbPeaksToKeep = (m_algoParams.m_expectedNbCenters > 0 ? m_algoParams.m_expectedNbCenters : static_cast(nbPeaks)); - nbPeaksToKeep = std::min(nbPeaksToKeep, static_cast(nbPeaks)); - for (int i = 0; i < nbPeaksToKeep; ++i) { - m_centerCandidatesList.push_back(merged_peaks_position_votes[i].m_position); - m_centerVotes.push_back(static_cast(merged_peaks_position_votes[i].m_votes)); - } - } -} - -void -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(static_cast(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. - std::vector > > votingPoints(nbBins); // Vectors that contain the points voting for each radius bin - - float rmin2 = m_algoParams.m_minRadius * m_algoParams.m_minRadius; - float rmax2 = m_algoParams.m_maxRadius * m_algoParams.m_maxRadius; - float circlePerfectness2 = m_algoParams.m_circlePerfectness * m_algoParams.m_circlePerfectness; - - for (size_t i = 0; i < nbCenterCandidates; ++i) { - std::pair centerCandidate = m_centerCandidatesList[i]; - // Initialize the radius accumulator of the candidate with 0s - radiusAccumList.clear(); - radiusAccumList.resize(nbBins, 0); - radiusActualValueList.clear(); - radiusActualValueList.resize(nbBins, 0.); - -#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98) - for (auto edgePoint : m_edgePointsList) -#else - const size_t nbEdgePoints = m_edgePointsList.size(); - for (size_t e = 0; e < nbEdgePoints; ++e) -#endif - { - // For each center candidate CeC_i, compute the distance with each edge point EP_j d_ij = dist(CeC_i; EP_j) -#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98) - float rx = edgePoint.second - centerCandidate.second; - float ry = edgePoint.first - centerCandidate.first; -#else - float rx = m_edgePointsList[e].second - centerCandidate.second; - float ry = m_edgePointsList[e].first - centerCandidate.first; -#endif - float r2 = (rx * rx) + (ry * ry); - if ((r2 > rmin2) && (r2 < rmax2)) { -#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98) - float gx = m_dIx[edgePoint.first][edgePoint.second]; - float gy = m_dIy[edgePoint.first][edgePoint.second]; -#else - float gx = m_dIx[m_edgePointsList[e].first][m_edgePointsList[e].second]; - float gy = m_dIy[m_edgePointsList[e].first][m_edgePointsList[e].second]; -#endif - float grad2 = (gx * gx) + (gy * gy); - - float scalProd = (rx * gx) + (ry * gy); - float scalProd2 = scalProd * scalProd; - if (scalProd2 >= (circlePerfectness2 * r2 * grad2)) { - // Look for the Radius Candidate Bin RCB_k to which d_ij is "the closest" will have an additionnal 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); - if ((r < (m_algoParams.m_minRadius + (m_algoParams.m_mergingRadiusDiffThresh * 0.5f))) - || (r >(m_algoParams.m_minRadius + (m_algoParams.m_mergingRadiusDiffThresh * (static_cast(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 - radiusAccumList[r_bin] += 1.f; - radiusActualValueList[r_bin] += r; - if (m_algoParams.m_recordVotingPoints) { -#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98) - votingPoints[r_bin].push_back(edgePoint); -#else - votingPoints[r_bin].push_back(m_edgePointsList[e]); -#endif - } - } - else { - float midRadiusPrevBin = m_algoParams.m_minRadius + (m_algoParams.m_mergingRadiusDiffThresh * ((r_bin - 1.f) + 0.5f)); - float midRadiusCurBin = m_algoParams.m_minRadius + (m_algoParams.m_mergingRadiusDiffThresh * (r_bin + 0.5f)); - float midRadiusNextBin = m_algoParams.m_minRadius + (m_algoParams.m_mergingRadiusDiffThresh * (r_bin + 1.f + 0.5f)); - - if ((r >= midRadiusCurBin) && (r <= midRadiusNextBin)) { - // The radius is at the end of the current bin or beginning of the next, we span the vote with the next bin - float voteCurBin = (midRadiusNextBin - r) / m_algoParams.m_mergingRadiusDiffThresh; // If the difference is big, it means that we are closer to the current bin - float voteNextBin = 1.f - voteCurBin; - radiusAccumList[r_bin] += voteCurBin; - radiusActualValueList[r_bin] += r * voteCurBin; - radiusAccumList[r_bin + 1] += voteNextBin; - radiusActualValueList[r_bin + 1] += r * voteNextBin; - if (m_algoParams.m_recordVotingPoints) { -#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98) - votingPoints[r_bin].push_back(edgePoint); - votingPoints[r_bin + 1].push_back(edgePoint); -#else - votingPoints[r_bin].push_back(m_edgePointsList[e]); - votingPoints[r_bin + 1].push_back(m_edgePointsList[e]); -#endif - } - } - else { - // The radius is at the end of the previous bin or beginning of the current, we span the vote with the previous bin - float votePrevBin = (r - midRadiusPrevBin) / m_algoParams.m_mergingRadiusDiffThresh; // If the difference is big, it means that we are closer to the previous bin - float voteCurBin = 1.f - votePrevBin; - radiusAccumList[r_bin] += voteCurBin; - radiusActualValueList[r_bin] += r * voteCurBin; - radiusAccumList[r_bin - 1] += votePrevBin; - radiusActualValueList[r_bin - 1] += r * votePrevBin; - if (m_algoParams.m_recordVotingPoints) { -#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98) - votingPoints[r_bin].push_back(edgePoint); - votingPoints[r_bin - 1].push_back(edgePoint); -#else - votingPoints[r_bin].push_back(m_edgePointsList[e]); - votingPoints[r_bin - 1].push_back(m_edgePointsList[e]); -#endif - } - } - } - } - } - } - -#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) - // Lambda to compute the effective radius (i.e. barycenter) of each radius bin - auto computeEffectiveRadius = [](const float &votes, const float &weigthedSumRadius) { - float r_effective = -1.f; - if (votes > std::numeric_limits::epsilon()) { - r_effective = weigthedSumRadius / votes; - } - return r_effective; - }; -#endif - - // Merging similar candidates - std::vector v_r_effective; // Vector of radius of each candidate after the merge step - std::vector v_votes_effective; // Vector of number of votes of each candidate after the merge step - std::vector > > v_votingPoints_effective; // Vector of voting points after the merge step - std::vector v_hasMerged_effective; // Vector indicating if merge has been performed for the different candidates - for (int idBin = 0; idBin < nbBins; ++idBin) { - float r_effective = computeEffectiveRadius(radiusAccumList[idBin], radiusActualValueList[idBin]); - float votes_effective = radiusAccumList[idBin]; - std::vector > votingPoints_effective = votingPoints[idBin]; - bool is_r_effective_similar = (r_effective > 0.f); - // Looking for potential similar radii in the following bins - // If so, compute the barycenter radius between them - int idCandidate = idBin + 1; - bool hasMerged = false; - while ((idCandidate < nbBins) && is_r_effective_similar) { - float r_effective_candidate = computeEffectiveRadius(radiusAccumList[idCandidate], radiusActualValueList[idCandidate]); - if (std::abs(r_effective_candidate - r_effective) < m_algoParams.m_mergingRadiusDiffThresh) { - r_effective = ((r_effective * votes_effective) + (r_effective_candidate * radiusAccumList[idCandidate])) / (votes_effective + radiusAccumList[idCandidate]); - votes_effective += radiusAccumList[idCandidate]; - radiusAccumList[idCandidate] = -.1f; - radiusActualValueList[idCandidate] = -1.f; - is_r_effective_similar = true; - if (m_algoParams.m_recordVotingPoints) { - // Move elements from votingPoints[idCandidate] to votingPoints_effective. - // votingPoints[idCandidate] is left in undefined but safe-to-destruct state. -#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98) - votingPoints_effective.insert( - votingPoints_effective.end(), - std::make_move_iterator(votingPoints[idCandidate].begin()), - std::make_move_iterator(votingPoints[idCandidate].end()) - ); -#else - votingPoints_effective.insert( - votingPoints_effective.end(), - votingPoints[idCandidate].begin(), - votingPoints[idCandidate].end() - ); -#endif - hasMerged = true; - } - } - else { - is_r_effective_similar = false; - } - ++idCandidate; - } - - if ((votes_effective > m_algoParams.m_centerMinThresh) && (votes_effective >= (m_algoParams.m_circleVisibilityRatioThresh * 2.f * M_PI_FLOAT * r_effective))) { - // Only the circles having enough votes and being visible enough are considered - v_r_effective.push_back(r_effective); - v_votes_effective.push_back(votes_effective); - if (m_algoParams.m_recordVotingPoints) { - v_votingPoints_effective.push_back(votingPoints_effective); - v_hasMerged_effective.push_back(hasMerged); - } - } - } - - unsigned int nbCandidates = static_cast(v_r_effective.size()); - for (unsigned int idBin = 0; idBin < nbCandidates; ++idBin) { - // If the circle of center CeC_i and radius RCB_k has enough votes, it is added to the list - // of Circle Candidates - float r_effective = v_r_effective[idBin]; - vpImageCircle candidateCircle(vpImagePoint(centerCandidate.first, centerCandidate.second), r_effective); - float proba = computeCircleProbability(candidateCircle, static_cast(v_votes_effective[idBin])); - if (proba > m_algoParams.m_circleProbaThresh) { - m_circleCandidates.push_back(candidateCircle); - m_circleCandidatesProbabilities.push_back(proba); - m_circleCandidatesVotes.push_back(static_cast(v_votes_effective[idBin])); - if (m_algoParams.m_recordVotingPoints) { - if (v_hasMerged_effective[idBin]) { - // Remove potential duplicated points - std::sort(v_votingPoints_effective[idBin].begin(), v_votingPoints_effective[idBin].end()); - v_votingPoints_effective[idBin].erase(std::unique(v_votingPoints_effective[idBin].begin(), v_votingPoints_effective[idBin].end()), v_votingPoints_effective[idBin].end()); - } - // Save the points - m_circleCandidatesVotingPoints.push_back(v_votingPoints_effective[idBin]); - } - } - } - } -} - -float -vpCircleHoughTransform::computeCircleProbability(const vpImageCircle &circle, const unsigned int &nbVotes) -{ - float proba(0.f); - float visibleArc(static_cast(nbVotes)); - float theoreticalLenght; - if (mp_mask != nullptr) { - theoreticalLenght = static_cast(circle.computePixelsInMask(*mp_mask)); - } - else { - theoreticalLenght = circle.computeArcLengthInRoI(vpRect(vpImagePoint(0, 0), m_edgeMap.getWidth(), m_edgeMap.getHeight())); - } - if (theoreticalLenght < std::numeric_limits::epsilon()) { - proba = 0.f; - } - else { - proba = std::min(visibleArc / theoreticalLenght, 1.f); - } - return proba; -} - -void -vpCircleHoughTransform::mergeCircleCandidates() -{ - std::vector circleCandidates = m_circleCandidates; - std::vector circleCandidatesVotes = m_circleCandidatesVotes; - std::vector circleCandidatesProba = m_circleCandidatesProbabilities; - std::vector > > circleCandidatesVotingPoints = m_circleCandidatesVotingPoints; - // First iteration of merge - mergeCandidates(circleCandidates, circleCandidatesVotes, circleCandidatesProba, circleCandidatesVotingPoints); - - // Second iteration of merge - mergeCandidates(circleCandidates, circleCandidatesVotes, circleCandidatesProba, circleCandidatesVotingPoints); - - // Saving the results - m_finalCircles = circleCandidates; - m_finalCircleVotes = circleCandidatesVotes; - m_finalCirclesProbabilities = circleCandidatesProba; - m_finalCirclesVotingPoints = circleCandidatesVotingPoints; -} - -void -vpCircleHoughTransform::mergeCandidates(std::vector &circleCandidates, std::vector &circleCandidatesVotes, - std::vector &circleCandidatesProba, std::vector > > &votingPoints) -{ - size_t nbCandidates = circleCandidates.size(); - size_t i = 0; - while (i < nbCandidates) { - vpImageCircle cic_i = circleCandidates[i]; - bool hasPerformedMerge = false; - // // For each other circle candidate CiC_j do: - size_t j = i + 1; - while (j < nbCandidates) { - vpImageCircle cic_j = circleCandidates[j]; - // // // Compute the similarity between CiC_i and CiC_j - double distanceBetweenCenters = vpImagePoint::distance(cic_i.getCenter(), cic_j.getCenter()); - double radiusDifference = std::abs(cic_i.getRadius() - cic_j.getRadius()); - bool areCirclesSimilar = ((distanceBetweenCenters < m_algoParams.m_centerMinDist) - && (radiusDifference < m_algoParams.m_mergingRadiusDiffThresh) - ); - - if (areCirclesSimilar) { - hasPerformedMerge = true; - // // // If the similarity exceeds a threshold, merge the circle candidates CiC_i and CiC_j and remove CiC_j of the list - unsigned int totalVotes = circleCandidatesVotes[i] + circleCandidatesVotes[j]; - float totalProba = circleCandidatesProba[i] + circleCandidatesProba[j]; - float newProba = 0.5f * totalProba; - float newRadius = ((cic_i.getRadius() * circleCandidatesProba[i]) + (cic_j.getRadius() * circleCandidatesProba[j])) / totalProba; - vpImagePoint newCenter = ((cic_i.getCenter() * circleCandidatesProba[i]) + (cic_j.getCenter() * circleCandidatesProba[j])) / totalProba; - cic_i = vpImageCircle(newCenter, newRadius); - circleCandidates[j] = circleCandidates[nbCandidates - 1]; - const unsigned int var2 = 2; - circleCandidatesVotes[i] = totalVotes / var2; // Compute the mean vote - circleCandidatesVotes[j] = circleCandidatesVotes[nbCandidates - 1]; - circleCandidatesProba[i] = newProba; - circleCandidatesProba[j] = circleCandidatesProba[nbCandidates - 1]; - if (m_algoParams.m_recordVotingPoints) { -#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98) - votingPoints[i].insert( - votingPoints[i].end(), - std::make_move_iterator(votingPoints[j].begin()), - std::make_move_iterator(votingPoints[j].end()) - ); -#else - votingPoints[i].insert( - votingPoints[i].end(), - votingPoints[j].begin(), - votingPoints[j].end() - ); -#endif - votingPoints.pop_back(); - } - circleCandidates.pop_back(); - circleCandidatesVotes.pop_back(); - circleCandidatesProba.pop_back(); - --nbCandidates; - // We do not update j because the new j-th candidate has not been evaluated yet - } - else { - // We will evaluate the next candidate - ++j; - } - } - // // Add the circle candidate CiC_i, potentially merged with other circle candidates, to the final list of detected circles - circleCandidates[i] = cic_i; - if (hasPerformedMerge && m_algoParams.m_recordVotingPoints) { - // Remove duplicated points - std::sort(votingPoints[i].begin(), votingPoints[i].end()); - votingPoints[i].erase(std::unique(votingPoints[i].begin(), votingPoints[i].end()), votingPoints[i].end()); - } - ++i; - } -} - -std::string -vpCircleHoughTransform::toString() const -{ - return m_algoParams.toString(); -} - -std::ostream & -operator<<(std::ostream &os, const vpCircleHoughTransform &detector) -{ - os << detector.toString(); - return os; -} - -END_VISP_NAMESPACE diff --git a/modules/imgproc/src/vpCircleHoughTransform_centers.cpp b/modules/imgproc/src/vpCircleHoughTransform_centers.cpp new file mode 100644 index 0000000000..62a09d7c79 --- /dev/null +++ b/modules/imgproc/src/vpCircleHoughTransform_centers.cpp @@ -0,0 +1,569 @@ +/* + * ViSP, open source Visual Servoing Platform software. + * Copyright (C) 2005 - 2024 by Inria. All rights reserved. + * + * This software is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * See the file LICENSE.txt at the root directory of this source + * distribution for additional information about the GNU GPL. + * + * For using ViSP with software that can not be combined with the GNU + * GPL, please contact Inria about acquiring a ViSP Professional + * Edition License. + * + * See https://visp.inria.fr for more information. + * + * This software was developed at: + * Inria Rennes - Bretagne Atlantique + * Campus Universitaire de Beaulieu + * 35042 Rennes Cedex + * France + * + * If you have questions regarding the use of this file, please contact + * Inria at visp@inria.fr + * + * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE + * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include +#include + +#include + +BEGIN_VISP_NAMESPACE + +namespace +{ + /** + * \brief Data required to update the center candidates accumulator along the + * gradient direction. + */ +typedef struct vpDataForAccumLoop +{ + unsigned int r; /*!< The row of the edge-point of interest.*/ + unsigned int c; /*!< The column of the edge-point of interest.*/ + float minRadius; /*!< The minimum radius of the searched circle.s.*/ + float maxRadius; /*!< The maximum radius of the searched circle.s.*/ + float minimumXpositionFloat; /*!< The minimum x-axis position of the center in the image.*/ + float minimumYpositionFloat; /*!< The minimum y-axis position of the center in the image.*/ + float maximumXpositionFloat; /*!< The maximum x-axis position of the center in the image.*/ + float maximumYpositionFloat; /*!< The maximum y-axis position of the center in the image.*/ + int offsetX; /*!< The offset to map the accumulator indices with the minimum x-axis value.*/ + int offsetY; /*!< The offset to map the accumulator indices with the minimum y-axis value.*/ + int accumulatorWidth; /*!< The width of the accumulator.*/ + int accumulatorHeight; /*!< The height of the accumulator.*/ +}vpDataForAccumLoop; + +/** + * \brief Store the coordinates for single step of update of the center candidates accumulator. + */ +typedef struct vpCoordinatesForAccumStep +{ + float x_orig; + float y_orig; + int x; + int y; +}vpCoordinatesForAccumStep; + +#if (VISP_CXX_STANDARD == VISP_CXX_STANDARD_98) +void updateAccumulator(const vpCoordinatesForAccumStep &coord, + const vpDataForAccumLoop &data, + vpImage &accum, bool &hasToStop) +{ + if (((coord.x - data.offsetX) < 0) || + ((coord.x - data.offsetX) >= data.accumulatorWidth) || + ((coord.y - data.offsetY) < 0) || + ((coord.y - data.offsetY) >= data.accumulatorHeight) + ) { + hasToStop = true; + } + else { + float dx = (coord.x_orig - static_cast(coord.x)); + float dy = (coord.y_orig - static_cast(coord.y)); + accum[coord.y - data.offsetY][coord.x - data.offsetX] += std::abs(dx) + std::abs(dy); + } +} + +bool sortingCenters(const vpCircleHoughTransform::vpCenterVotes &position_vote_a, + const vpCircleHoughTransform::vpCenterVotes &position_vote_b) +{ + return position_vote_a.m_votes > position_vote_b.m_votes; +} +#endif + +/** + * \brief Update the center accumulator along the positive and negative gradient direction + * starting from an edge-point of interest. + * + * \param[in] data The data required for the algorithm. + * \param[out] sx The gradient along x. + * \param[out] sy The gradient along y. + * \param[out] centersAccum The center candidates accumulator. + */ +void +updateAccumAlongGradientDir(const vpDataForAccumLoop &data, float &sx, float &sy, vpImage ¢ersAccum) +{ + static const int nbDirections = 2; + for (int k1 = 0; k1 < nbDirections; ++k1) { + bool hasToStopLoop = false; + int x_low_prev = std::numeric_limits::max(), y_low_prev, y_high_prev; + int x_high_prev = (y_low_prev = (y_high_prev = x_low_prev)); + + float rstart = data.minRadius, rstop = data.maxRadius; + float min_minus_c = data.minimumXpositionFloat - static_cast(data.c); + float min_minus_r = data.minimumYpositionFloat - static_cast(data.r); + float max_minus_c = data.maximumXpositionFloat - static_cast(data.c); + float max_minus_r = data.maximumYpositionFloat - static_cast(data.r); + if (sx > 0) { + float rmin = min_minus_c / sx; + rstart = std::max(rmin, data.minRadius); + float rmax = max_minus_c / sx; + rstop = std::min(rmax, data.maxRadius); + } + else if (sx < 0) { + float rmin = max_minus_c / sx; + rstart = std::max(rmin, data.minRadius); + float rmax = min_minus_c / sx; + rstop = std::min(rmax, data.maxRadius); + } + + if (sy > 0) { + float rmin = min_minus_r / sy; + rstart = std::max(rmin, rstart); + float rmax = max_minus_r / sy; + rstop = std::min(rmax, rstop); + } + else if (sy < 0) { + float rmin = max_minus_r / sy; + rstart = std::max(rmin, rstart); + float rmax = min_minus_r / sy; + rstop = std::min(rmax, rstop); + } + + float deltar_x = 1.f / std::abs(sx), deltar_y = 1.f / std::abs(sy); + float deltar = std::min(deltar_x, deltar_y); + + float rad = rstart; + while ((rad <= rstop) && (!hasToStopLoop)) { + float x1 = static_cast(data.c) + (rad * sx); + float y1 = static_cast(data.r) + (rad * sy); + rad += deltar; // Update rad that is not used below not to forget it + + bool xOutsideRoI = (x1 < data.minimumXpositionFloat) || (x1 > data.maximumXpositionFloat); + bool yOutsideRoI = (y1 < data.minimumYpositionFloat) || (y1 > data.maximumYpositionFloat); + // Continue only if the center is inside the search region. + if (!(xOutsideRoI || yOutsideRoI)) { + int x_low, x_high, y_low, y_high; + + if (x1 > 0.) { + x_low = static_cast(std::floor(x1)); + x_high = static_cast(std::ceil(x1)); + } + else { + x_low = -(static_cast(std::ceil(-x1))); + x_high = -(static_cast(std::floor(-x1))); + } + + if (y1 > 0.) { + y_low = static_cast(std::floor(y1)); + y_high = static_cast(std::ceil(y1)); + } + else { + y_low = -(static_cast(std::ceil(-1. * y1))); + y_high = -(static_cast(std::floor(-1. * y1))); + } + + bool xHasNotChanged = (x_low_prev == x_low) && (x_high_prev == x_high); + bool yHasNotChanged = (y_low_prev == y_low) && (y_high_prev == y_high); + + // Avoid duplicated votes to the same center candidate + if (!(xHasNotChanged && yHasNotChanged)) { + x_low_prev = x_low; + x_high_prev = x_high; + y_low_prev = y_low; + y_high_prev = y_high; + +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) + auto updateAccumulator = + [](const vpCoordinatesForAccumStep &coord, + const vpDataForAccumLoop &data, + vpImage &accum, bool &hasToStop) + { + if (((coord.x - data.offsetX) < 0) || + ((coord.x - data.offsetX) >= data.accumulatorWidth) || + ((coord.y - data.offsetY) < 0) || + ((coord.y - data.offsetY) >= data.accumulatorHeight) + ) { + hasToStop = true; + } + else { + float dx = (coord.x_orig - static_cast(coord.x)); + float dy = (coord.y_orig - static_cast(coord.y)); + accum[coord.y - data.offsetY][coord.x - data.offsetX] += std::abs(dx) + std::abs(dy); + } + }; +#endif + vpCoordinatesForAccumStep coords; + coords.x_orig = x1; + coords.y_orig = y1; + coords.x = x_low; + coords.y = y_low; + updateAccumulator(coords, data, centersAccum, hasToStopLoop); + + coords.x = x_high; + coords.y = y_high; + updateAccumulator(coords, data, centersAccum, hasToStopLoop); + } + } + } + sx = -sx; + sy = -sy; +} +} +} + + +// Static variables +const unsigned char vpCircleHoughTransform::edgeMapOn = 255; +const unsigned char vpCircleHoughTransform::edgeMapOff = 0; + +void +vpCircleHoughTransform::computeGradients(const vpImage &I) +{ + if ((m_algoParams.m_filteringAndGradientType == vpImageFilter::CANNY_GBLUR_SOBEL_FILTERING) + || (m_algoParams.m_filteringAndGradientType == vpImageFilter::CANNY_GBLUR_SCHARR_FILTERING)) { + // Computing the Gaussian blurr + vpImage Iblur, GIx; + vpImageFilter::filterX(I, GIx, m_fg.data, m_algoParams.m_gaussianKernelSize, mp_mask); + vpImageFilter::filterY(GIx, Iblur, m_fg.data, m_algoParams.m_gaussianKernelSize, mp_mask); + + // Computing the gradients + vpImageFilter::filter(Iblur, m_dIx, m_gradientFilterX, true, mp_mask); + vpImageFilter::filter(Iblur, m_dIy, m_gradientFilterY, true, mp_mask); + } + else { + std::string errMsg("[computeGradients] The filtering + gradient operators \""); + errMsg += vpImageFilter::vpCannyFiltAndGradTypeToStr(m_algoParams.m_filteringAndGradientType); + errMsg += "\" is not implemented (yet)."; + throw(vpException(vpException::notImplementedError, errMsg)); + } +} + +void +vpCircleHoughTransform::edgeDetection(const vpImage &I) +{ + if (m_algoParams.m_cannyBackendType == vpImageFilter::CANNY_VISP_BACKEND) { + // This is done to increase the time performances, because it avoids to + // recompute the gradient in the vpImageFilter::canny method + m_cannyVisp.setFilteringAndGradientType(m_algoParams.m_filteringAndGradientType); + m_cannyVisp.setCannyThresholds(m_algoParams.m_lowerCannyThresh, m_algoParams.m_upperCannyThresh); + m_cannyVisp.setCannyThresholdsRatio(m_algoParams.m_lowerCannyThreshRatio, m_algoParams.m_upperCannyThreshRatio); + m_cannyVisp.setGradients(m_dIx, m_dIy); + m_cannyVisp.setMask(mp_mask); + m_edgeMap = m_cannyVisp.detect(I); + } + else { + if (mp_mask != nullptr) { + // Delete pixels that fall outside the mask + vpImage I_masked(I); + unsigned int nbRows = I_masked.getHeight(); + unsigned int nbCols = I_masked.getWidth(); + for (unsigned int r = 0; r < nbRows; ++r) { + for (unsigned int c = 0; c < nbCols; ++c) { + if (!((*mp_mask)[r][c])) { + I_masked[r][c] = 0; + } + } + } + + // We will have to recompute the gradient in the desired backend format anyway so we let + // the vpImageFilter::canny method take care of it + vpImageFilter::canny(I_masked, m_edgeMap, m_algoParams.m_gaussianKernelSize, m_algoParams.m_lowerCannyThresh, + m_algoParams.m_upperCannyThresh, m_algoParams.m_gradientFilterKernelSize, m_algoParams.m_gaussianStdev, + m_algoParams.m_lowerCannyThreshRatio, m_algoParams.m_upperCannyThreshRatio, true, + m_algoParams.m_cannyBackendType, m_algoParams.m_filteringAndGradientType); + } + else { + vpImageFilter::canny(I, m_edgeMap, m_algoParams.m_gaussianKernelSize, m_algoParams.m_lowerCannyThresh, + m_algoParams.m_upperCannyThresh, m_algoParams.m_gradientFilterKernelSize, m_algoParams.m_gaussianStdev, + m_algoParams.m_lowerCannyThreshRatio, m_algoParams.m_upperCannyThreshRatio, true, + m_algoParams.m_cannyBackendType, m_algoParams.m_filteringAndGradientType); + } + } + + for (int i = 0; i < m_algoParams.m_edgeMapFilteringNbIter; ++i) { + filterEdgeMap(); + } +} + +void +vpCircleHoughTransform::filterEdgeMap() +{ + vpImage J = m_edgeMap; + const unsigned int height = J.getHeight(); + const unsigned int width = J.getWidth(); + const int minNbContiguousPts = 2; + + for (unsigned int i = 1; i < (height - 1); ++i) { + for (unsigned int j = 1; j < (width - 1); ++j) { + if (J[i][j] == vpCircleHoughTransform::edgeMapOn) { + // Consider 8 neighbors + int topLeftPixel = static_cast(J[i - 1][j - 1]); + int topPixel = static_cast(J[i - 1][j]); + int topRightPixel = static_cast(J[i - 1][j + 1]); + int botLeftPixel = static_cast(J[i + 1][j - 1]); + int bottomPixel = static_cast(J[i + 1][j]); + int botRightPixel = static_cast(J[i + 1][j + 1]); + int leftPixel = static_cast(J[i][j - 1]); + int rightPixel = static_cast(J[i][j + 1]); + if ((topLeftPixel + topPixel + topRightPixel + + botLeftPixel + bottomPixel + botRightPixel + + leftPixel + rightPixel + ) >= (minNbContiguousPts * static_cast(vpCircleHoughTransform::edgeMapOn))) { + // At least minNbContiguousPts of the 8-neighbor points are also an edge point + // so we keep the edge point + m_edgeMap[i][j] = vpCircleHoughTransform::edgeMapOn; + } + else { + // The edge point is isolated => we erase it + m_edgeMap[i][j] = vpCircleHoughTransform::edgeMapOff; + } + } + } + } +} + +void +vpCircleHoughTransform::computeCenterCandidates() +{ + // For each edge point EP_i, check the image gradient at EP_i + // Then, for each image point in the direction of the gradient, + // increment the accumulator + // We can perform bilinear interpolation in order not to vote for a "line" of + // points, but for an "area" of points + unsigned int nbRows = m_edgeMap.getRows(), nbCols = m_edgeMap.getCols(); + + // Computing the minimum and maximum horizontal position of the center candidates + // 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); + float minimumXpositionFloat = static_cast(minimumXposition); + float maximumXpositionFloat = static_cast(maximumXposition); + int offsetX = minimumXposition; + int accumulatorWidth = (maximumXposition - minimumXposition) + 1; + if (accumulatorWidth <= 0) { + throw(vpException(vpException::dimensionError, "[vpCircleHoughTransform::computeCenterCandidates] Accumulator width <= 0!")); + } + + // Computing the minimum and maximum vertical position of the center candidates + // 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 + nbRows)); + minimumYposition = std::min(minimumYposition, maximumYposition - 1); + float minimumYpositionFloat = static_cast(minimumYposition); + float maximumYpositionFloat = static_cast(maximumYposition); + int offsetY = minimumYposition; + int accumulatorHeight = (maximumYposition - minimumYposition) + 1; + if (accumulatorHeight <= 0) { + throw(vpException(vpException::dimensionError, "[vpCircleHoughTransform::computeCenterCandidates] Accumulator height <= 0!")); + } + + vpImage centersAccum(accumulatorHeight, accumulatorWidth + 1, 0.); /*!< Votes for the center candidates.*/ + + for (unsigned int r = 0; r < nbRows; ++r) { + for (unsigned int c = 0; c < nbCols; ++c) { + if (m_edgeMap[r][c] == vpCircleHoughTransform::edgeMapOn) { + // Voting for points in both direction of the gradient + // Step from min_radius to max_radius in both directions of the gradient + float mag = std::sqrt((m_dIx[r][c] * m_dIx[r][c]) + (m_dIy[r][c] * m_dIy[r][c])); + + float sx = 0.f, sy = 0.f; + if (std::abs(mag) >= std::numeric_limits::epsilon()) { + sx = m_dIx[r][c] / mag; + sy = m_dIy[r][c] / mag; + + // Saving the edge point for further use + m_edgePointsList.push_back(std::pair(r, c)); + + vpDataForAccumLoop data; + data.accumulatorHeight = accumulatorHeight; + data.accumulatorWidth = accumulatorWidth; + data.c = c; + data.maximumXpositionFloat = maximumXpositionFloat; + data.maximumYpositionFloat = maximumYpositionFloat; + data.maxRadius = m_algoParams.m_maxRadius; + data.minimumXpositionFloat = minimumXpositionFloat; + data.minimumYpositionFloat = minimumYpositionFloat; + data.minRadius = m_algoParams.m_minRadius; + data.offsetX = offsetX; + data.offsetY = offsetY; + data.r = r; + updateAccumAlongGradientDir(data, sx, sy, centersAccum); + } + } + } + } + + // 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 + vpImageMorphology::dilatation(centerCandidatesMaxima, dilatationKernelSize); + + // Look for the image points that correspond to the accumulator maxima + // These points will become the center candidates + // find the possible circle centers + int nbColsAccum = centersAccum.getCols(); + int nbRowsAccum = centersAccum.getRows(); + int nbVotes = -1; + std::vector peak_positions_votes; + + for (int y = 0; y < nbRowsAccum; ++y) { + int left = -1; + for (int x = 0; x < nbColsAccum; ++x) { + if ((centersAccum[y][x] >= m_algoParams.m_centerMinThresh) + && (vpMath::equal(centersAccum[y][x], centerCandidatesMaxima[y][x])) + && (centersAccum[y][x] > centersAccum[y][x + 1]) + ) { + if (left < 0) { + left = 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., 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); + for (int r = startingRow; r < endRow; ++r) { + for (int c = startingCol; c < endCol; ++c) { + sumVotes += centersAccum[r][c]; + x_avg += centersAccum[r][c] * c; + y_avg += centersAccum[r][c] * r; + } + } + float avgVotes = sumVotes / static_cast(m_algoParams.m_averagingWindowSize * m_algoParams.m_averagingWindowSize); + if (avgVotes > m_algoParams.m_centerMinThresh) { + x_avg /= static_cast(sumVotes); + y_avg /= static_cast(sumVotes); + std::pair position(y_avg + static_cast(offsetY), x_avg + static_cast(offsetX)); + vpCenterVotes position_vote; + position_vote.m_position = position; + position_vote.m_votes = avgVotes; + peak_positions_votes.push_back(position_vote); + } + if (nbVotes < 0) { + std::stringstream errMsg; + errMsg << "nbVotes (" << nbVotes << ") < 0, thresh = " << m_algoParams.m_centerMinThresh; + throw(vpException(vpException::badValue, errMsg.str())); + } + left = -1; + nbVotes = -1; + } + } + } + filterCenterCandidates(peak_positions_votes); +} + +void +vpCircleHoughTransform::filterCenterCandidates(const std::vector &peak_positions_votes) +{ + unsigned int nbPeaks = static_cast(peak_positions_votes.size()); + if (nbPeaks > 0) { + std::vector has_been_merged(nbPeaks, false); + std::vector merged_peaks_position_votes; + float squared_distance_max = m_algoParams.m_centerMinDist * m_algoParams.m_centerMinDist; + for (unsigned int idPeak = 0; idPeak < nbPeaks; ++idPeak) { + float votes = peak_positions_votes[idPeak].m_votes; + // Ignoring peak that has already been merged + if (!has_been_merged[idPeak]) { + if (votes < m_algoParams.m_centerMinThresh) { + // Ignoring peak whose number of votes is lower than the threshold + has_been_merged[idPeak] = true; + } + else { + vpCentersBarycenter barycenter = mergeSimilarCenters(idPeak, nbPeaks, squared_distance_max, peak_positions_votes, has_been_merged); + + float avg_votes = barycenter.m_totalVotes / barycenter.m_nbElectors; + // Only the centers having enough votes are considered + if (avg_votes > m_algoParams.m_centerMinThresh) { + barycenter.m_position.first /= barycenter.m_totalVotes; + barycenter.m_position.second /= barycenter.m_totalVotes; + vpCenterVotes barycenter_votes; + barycenter_votes.m_position = barycenter.m_position; + barycenter_votes.m_votes = avg_votes; + merged_peaks_position_votes.push_back(barycenter_votes); + } + } + } + } + +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) + auto sortingCenters = [](const vpCenterVotes &position_vote_a, + const vpCenterVotes &position_vote_b) { + return position_vote_a.m_votes > position_vote_b.m_votes; + }; +#endif + + std::sort(merged_peaks_position_votes.begin(), merged_peaks_position_votes.end(), sortingCenters); + + nbPeaks = static_cast(merged_peaks_position_votes.size()); + int nbPeaksToKeep = (m_algoParams.m_expectedNbCenters > 0 ? m_algoParams.m_expectedNbCenters : static_cast(nbPeaks)); + nbPeaksToKeep = std::min(nbPeaksToKeep, static_cast(nbPeaks)); + for (int i = 0; i < nbPeaksToKeep; ++i) { + m_centerCandidatesList.push_back(merged_peaks_position_votes[i].m_position); + m_centerVotes.push_back(static_cast(merged_peaks_position_votes[i].m_votes)); + } + } + } + +vpCircleHoughTransform::vpCentersBarycenter +vpCircleHoughTransform::mergeSimilarCenters(const unsigned int &idPeak, const unsigned int &nbPeaks, const float &squared_distance_max, const std::vector &peak_positions_votes, std::vector &has_been_merged) +{ + std::pair position = peak_positions_votes[idPeak].m_position; + vpCentersBarycenter barycenter; + barycenter.m_position.first = position.first * peak_positions_votes[idPeak].m_votes; + barycenter.m_position.second = position.second * peak_positions_votes[idPeak].m_votes; + barycenter.m_totalVotes = peak_positions_votes[idPeak].m_votes; + barycenter.m_nbElectors = 1.f; + // Looking for potential similar peak in the following peaks + for (unsigned int idCandidate = idPeak + 1; idCandidate < nbPeaks; ++idCandidate) { + float votes_candidate = peak_positions_votes[idCandidate].m_votes; + // Ignoring peaks that have already been merged + if (!has_been_merged[idCandidate]) { + if (votes_candidate < m_algoParams.m_centerMinThresh) { + // Ignoring peak whose number of votes is lower than the threshold + has_been_merged[idCandidate] = true; + } + else { + // Computing the distance with the peak of insterest + std::pair position_candidate = peak_positions_votes[idCandidate].m_position; + float squared_distance = ((position.first - position_candidate.first) * (position.first - position_candidate.first)) + + ((position.second - position_candidate.second) * (position.second - position_candidate.second)); + + // If the peaks are similar, update the barycenter peak between them and corresponding votes + if (squared_distance < squared_distance_max) { + barycenter.m_position.first += position_candidate.first * votes_candidate; + barycenter.m_position.second += position_candidate.second * votes_candidate; + barycenter.m_totalVotes += votes_candidate; + barycenter.m_nbElectors += 1.f; + has_been_merged[idCandidate] = true; + } + } + } + } + return barycenter; +} + +END_VISP_NAMESPACE diff --git a/modules/imgproc/src/vpCircleHoughTransform_circles.cpp b/modules/imgproc/src/vpCircleHoughTransform_circles.cpp new file mode 100644 index 0000000000..844f853027 --- /dev/null +++ b/modules/imgproc/src/vpCircleHoughTransform_circles.cpp @@ -0,0 +1,387 @@ +/* + * ViSP, open source Visual Servoing Platform software. + * Copyright (C) 2005 - 2024 by Inria. All rights reserved. + * + * This software is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * See the file LICENSE.txt at the root directory of this source + * distribution for additional information about the GNU GPL. + * + * For using ViSP with software that can not be combined with the GNU + * GPL, please contact Inria about acquiring a ViSP Professional + * Edition License. + * + * See https://visp.inria.fr for more information. + * + * This software was developed at: + * Inria Rennes - Bretagne Atlantique + * Campus Universitaire de Beaulieu + * 35042 Rennes Cedex + * France + * + * If you have questions regarding the use of this file, please contact + * Inria at visp@inria.fr + * + * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE + * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include +#include + +#include + +BEGIN_VISP_NAMESPACE + +namespace +{ +#if (VISP_CXX_STANDARD == VISP_CXX_STANDARD_98) +float computeEffectiveRadius(const float &votes, const float &weigthedSumRadius) +{ + float r_effective = -1.f; + if (votes > std::numeric_limits::epsilon()) { + r_effective = weigthedSumRadius / votes; + } + return r_effective; +} +#endif + +typedef struct vpDataUpdateRadAccum +{ + int m_nbBins; + std::pair m_centerCandidate; + std::pair m_edgePoint; + const vpImage &m_dIx; + const vpImage &m_dIy; + float m_rx; + float m_ry; + float m_circlePerfectness2; + float m_r2; + float m_minRadius; + float m_mergingRadiusDiffThresh; + bool m_recordVotingPoints; + + vpDataUpdateRadAccum(const vpImage &dIx, const vpImage &dIy) + : m_dIx(dIx) + , m_dIy(dIy) + { } +} vpDataUpdateRadAccum; + +void +updateRadiusAccumulator(const vpDataUpdateRadAccum &data, std::vector &radiusAccumList, + std::vector &radiusActualValueList, + std::vector > > &votingPoints) +{ + float gx = data.m_dIx[data.m_edgePoint.first][data.m_edgePoint.second]; + float gy = data.m_dIy[data.m_edgePoint.first][data.m_edgePoint.second]; + float grad2 = (gx * gx) + (gy * gy); + float scalProd = (data.m_rx * gx) + (data.m_ry * gy); + float scalProd2 = scalProd * scalProd; + if (scalProd2 >= (data.m_circlePerfectness2 * data.m_r2 * grad2)) { + // Look for the Radius Candidate Bin RCB_k to which d_ij is "the closest" will have an additionnal vote + float r = static_cast(std::sqrt(data.m_r2)); + int r_bin = static_cast(std::floor((r - data.m_minRadius) / data.m_mergingRadiusDiffThresh)); + r_bin = std::min(r_bin, data.m_nbBins - 1); + if ((r < (data.m_minRadius + (data.m_mergingRadiusDiffThresh * 0.5f))) + || (r >(data.m_minRadius + (data.m_mergingRadiusDiffThresh * (static_cast(data.m_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 + radiusAccumList[r_bin] += 1.f; + radiusActualValueList[r_bin] += r; + if (data.m_recordVotingPoints) { + votingPoints[r_bin].push_back(data.m_edgePoint); + } + } + else { + float midRadiusPrevBin = data.m_minRadius + (data.m_mergingRadiusDiffThresh * ((r_bin - 1.f) + 0.5f)); + float midRadiusCurBin = data.m_minRadius + (data.m_mergingRadiusDiffThresh * (r_bin + 0.5f)); + float midRadiusNextBin = data.m_minRadius + (data.m_mergingRadiusDiffThresh * (r_bin + 1.f + 0.5f)); + + if ((r >= midRadiusCurBin) && (r <= midRadiusNextBin)) { + // The radius is at the end of the current bin or beginning of the next, we span the vote with the next bin + float voteCurBin = (midRadiusNextBin - r) / data.m_mergingRadiusDiffThresh; // If the difference is big, it means that we are closer to the current bin + float voteNextBin = 1.f - voteCurBin; + radiusAccumList[r_bin] += voteCurBin; + radiusActualValueList[r_bin] += r * voteCurBin; + radiusAccumList[r_bin + 1] += voteNextBin; + radiusActualValueList[r_bin + 1] += r * voteNextBin; + if (data.m_recordVotingPoints) { + votingPoints[r_bin].push_back(data.m_edgePoint); + votingPoints[r_bin + 1].push_back(data.m_edgePoint); + } + } + else { + // The radius is at the end of the previous bin or beginning of the current, we span the vote with the previous bin + float votePrevBin = (r - midRadiusPrevBin) / data.m_mergingRadiusDiffThresh; // If the difference is big, it means that we are closer to the previous bin + float voteCurBin = 1.f - votePrevBin; + radiusAccumList[r_bin] += voteCurBin; + radiusActualValueList[r_bin] += r * voteCurBin; + radiusAccumList[r_bin - 1] += votePrevBin; + radiusActualValueList[r_bin - 1] += r * votePrevBin; + if (data.m_recordVotingPoints) { + votingPoints[r_bin].push_back(data.m_edgePoint); + votingPoints[r_bin - 1].push_back(data.m_edgePoint); + } + } + } + } +} +} + +void +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(static_cast(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. + std::vector > > votingPoints(nbBins); // Vectors that contain the points voting for each radius bin + + float rmin2 = m_algoParams.m_minRadius * m_algoParams.m_minRadius; + float rmax2 = m_algoParams.m_maxRadius * m_algoParams.m_maxRadius; + float circlePerfectness2 = m_algoParams.m_circlePerfectness * m_algoParams.m_circlePerfectness; + + vpDataUpdateRadAccum data(m_dIx, m_dIy); + data.m_nbBins = nbBins; + data.m_circlePerfectness2 = circlePerfectness2; + data.m_minRadius = m_algoParams.m_minRadius; + data.m_mergingRadiusDiffThresh = m_algoParams.m_mergingRadiusDiffThresh; + data.m_recordVotingPoints = m_algoParams.m_recordVotingPoints; + + for (size_t i = 0; i < nbCenterCandidates; ++i) { + std::pair centerCandidate = m_centerCandidatesList[i]; + // Initialize the radius accumulator of the candidate with 0s + radiusAccumList.clear(); + radiusAccumList.resize(nbBins, 0); + radiusActualValueList.clear(); + radiusActualValueList.resize(nbBins, 0.); + + const unsigned int nbEdgePoints = m_edgePointsList.size(); + for (unsigned int e = 0; e < nbEdgePoints; ++e) { + const std::pair &edgePoint = m_edgePointsList[e]; + + // For each center candidate CeC_i, compute the distance with each edge point EP_j d_ij = dist(CeC_i; EP_j) + float rx = edgePoint.second - centerCandidate.second; + float ry = edgePoint.first - centerCandidate.first; + float r2 = (rx * rx) + (ry * ry); + if ((r2 > rmin2) && (r2 < rmax2)) { + data.m_centerCandidate = centerCandidate; + data.m_edgePoint = edgePoint; + data.m_rx = rx; + data.m_ry = ry; + data.m_r2 = r2; + updateRadiusAccumulator(data, radiusAccumList, radiusActualValueList, votingPoints); + } + } + +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) + // Lambda to compute the effective radius (i.e. barycenter) of each radius bin + auto computeEffectiveRadius = [](const float &votes, const float &weigthedSumRadius) { + float r_effective = -1.f; + if (votes > std::numeric_limits::epsilon()) { + r_effective = weigthedSumRadius / votes; + } + return r_effective; + }; +#endif + + // Merging similar candidates + std::vector v_r_effective; // Vector of radius of each candidate after the merge step + std::vector v_votes_effective; // Vector of number of votes of each candidate after the merge step + std::vector > > v_votingPoints_effective; // Vector of voting points after the merge step + std::vector v_hasMerged_effective; // Vector indicating if merge has been performed for the different candidates + for (int idBin = 0; idBin < nbBins; ++idBin) { + float r_effective = computeEffectiveRadius(radiusAccumList[idBin], radiusActualValueList[idBin]); + float votes_effective = radiusAccumList[idBin]; + std::vector > votingPoints_effective = votingPoints[idBin]; + bool is_r_effective_similar = (r_effective > 0.f); + // Looking for potential similar radii in the following bins + // If so, compute the barycenter radius between them + int idCandidate = idBin + 1; + bool hasMerged = false; + while ((idCandidate < nbBins) && is_r_effective_similar) { + float r_effective_candidate = computeEffectiveRadius(radiusAccumList[idCandidate], radiusActualValueList[idCandidate]); + if (std::abs(r_effective_candidate - r_effective) < m_algoParams.m_mergingRadiusDiffThresh) { + r_effective = ((r_effective * votes_effective) + (r_effective_candidate * radiusAccumList[idCandidate])) / (votes_effective + radiusAccumList[idCandidate]); + votes_effective += radiusAccumList[idCandidate]; + radiusAccumList[idCandidate] = -.1f; + radiusActualValueList[idCandidate] = -1.f; + is_r_effective_similar = true; + if (m_algoParams.m_recordVotingPoints) { + // Move elements from votingPoints[idCandidate] to votingPoints_effective. + // votingPoints[idCandidate] is left in undefined but safe-to-destruct state. +#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98) + votingPoints_effective.insert( + votingPoints_effective.end(), + std::make_move_iterator(votingPoints[idCandidate].begin()), + std::make_move_iterator(votingPoints[idCandidate].end()) + ); +#else + votingPoints_effective.insert( + votingPoints_effective.end(), + votingPoints[idCandidate].begin(), + votingPoints[idCandidate].end() + ); +#endif + hasMerged = true; + } + } + else { + is_r_effective_similar = false; + } + ++idCandidate; + } + + if ((votes_effective > m_algoParams.m_centerMinThresh) && (votes_effective >= (m_algoParams.m_circleVisibilityRatioThresh * 2.f * M_PI_FLOAT * r_effective))) { + // Only the circles having enough votes and being visible enough are considered + v_r_effective.push_back(r_effective); + v_votes_effective.push_back(votes_effective); + if (m_algoParams.m_recordVotingPoints) { + v_votingPoints_effective.push_back(votingPoints_effective); + v_hasMerged_effective.push_back(hasMerged); + } + } + } + + unsigned int nbCandidates = static_cast(v_r_effective.size()); + for (unsigned int idBin = 0; idBin < nbCandidates; ++idBin) { + // If the circle of center CeC_i and radius RCB_k has enough votes, it is added to the list + // of Circle Candidates + float r_effective = v_r_effective[idBin]; + vpImageCircle candidateCircle(vpImagePoint(centerCandidate.first, centerCandidate.second), r_effective); + float proba = computeCircleProbability(candidateCircle, static_cast(v_votes_effective[idBin])); + if (proba > m_algoParams.m_circleProbaThresh) { + m_circleCandidates.push_back(candidateCircle); + m_circleCandidatesProbabilities.push_back(proba); + m_circleCandidatesVotes.push_back(static_cast(v_votes_effective[idBin])); + if (m_algoParams.m_recordVotingPoints) { + if (v_hasMerged_effective[idBin]) { + // Remove potential duplicated points + std::sort(v_votingPoints_effective[idBin].begin(), v_votingPoints_effective[idBin].end()); + v_votingPoints_effective[idBin].erase(std::unique(v_votingPoints_effective[idBin].begin(), v_votingPoints_effective[idBin].end()), v_votingPoints_effective[idBin].end()); + } + // Save the points + m_circleCandidatesVotingPoints.push_back(v_votingPoints_effective[idBin]); + } + } + } + } +} + +float +vpCircleHoughTransform::computeCircleProbability(const vpImageCircle &circle, const unsigned int &nbVotes) +{ + float proba(0.f); + float visibleArc(static_cast(nbVotes)); + float theoreticalLenght; + if (mp_mask != nullptr) { + theoreticalLenght = static_cast(circle.computePixelsInMask(*mp_mask)); + } + else { + theoreticalLenght = circle.computeArcLengthInRoI(vpRect(vpImagePoint(0, 0), m_edgeMap.getWidth(), m_edgeMap.getHeight())); + } + if (theoreticalLenght < std::numeric_limits::epsilon()) { + proba = 0.f; + } + else { + proba = std::min(visibleArc / theoreticalLenght, 1.f); + } + return proba; +} + +void +vpCircleHoughTransform::mergeCircleCandidates() +{ + std::vector circleCandidates = m_circleCandidates; + std::vector circleCandidatesVotes = m_circleCandidatesVotes; + std::vector circleCandidatesProba = m_circleCandidatesProbabilities; + std::vector > > circleCandidatesVotingPoints = m_circleCandidatesVotingPoints; + // First iteration of merge + mergeCandidates(circleCandidates, circleCandidatesVotes, circleCandidatesProba, circleCandidatesVotingPoints); + + // Second iteration of merge + mergeCandidates(circleCandidates, circleCandidatesVotes, circleCandidatesProba, circleCandidatesVotingPoints); + + // Saving the results + m_finalCircles = circleCandidates; + m_finalCircleVotes = circleCandidatesVotes; + m_finalCirclesProbabilities = circleCandidatesProba; + m_finalCirclesVotingPoints = circleCandidatesVotingPoints; +} + +void +vpCircleHoughTransform::mergeCandidates(std::vector &circleCandidates, std::vector &circleCandidatesVotes, + std::vector &circleCandidatesProba, std::vector > > &votingPoints) +{ + size_t nbCandidates = circleCandidates.size(); + size_t i = 0; + while (i < nbCandidates) { + vpImageCircle cic_i = circleCandidates[i]; + bool hasPerformedMerge = false; + // // For each other circle candidate CiC_j do: + size_t j = i + 1; + while (j < nbCandidates) { + vpImageCircle cic_j = circleCandidates[j]; + // // // Compute the similarity between CiC_i and CiC_j + double distanceBetweenCenters = vpImagePoint::distance(cic_i.getCenter(), cic_j.getCenter()); + double radiusDifference = std::abs(cic_i.getRadius() - cic_j.getRadius()); + bool areCirclesSimilar = ((distanceBetweenCenters < m_algoParams.m_centerMinDist) + && (radiusDifference < m_algoParams.m_mergingRadiusDiffThresh) + ); + + if (areCirclesSimilar) { + hasPerformedMerge = true; + // // // If the similarity exceeds a threshold, merge the circle candidates CiC_i and CiC_j and remove CiC_j of the list + unsigned int totalVotes = circleCandidatesVotes[i] + circleCandidatesVotes[j]; + float totalProba = circleCandidatesProba[i] + circleCandidatesProba[j]; + float newProba = 0.5f * totalProba; + float newRadius = ((cic_i.getRadius() * circleCandidatesProba[i]) + (cic_j.getRadius() * circleCandidatesProba[j])) / totalProba; + vpImagePoint newCenter = ((cic_i.getCenter() * circleCandidatesProba[i]) + (cic_j.getCenter() * circleCandidatesProba[j])) / totalProba; + cic_i = vpImageCircle(newCenter, newRadius); + circleCandidates[j] = circleCandidates[nbCandidates - 1]; + const unsigned int var2 = 2; + circleCandidatesVotes[i] = totalVotes / var2; // Compute the mean vote + circleCandidatesVotes[j] = circleCandidatesVotes[nbCandidates - 1]; + circleCandidatesProba[i] = newProba; + circleCandidatesProba[j] = circleCandidatesProba[nbCandidates - 1]; + if (m_algoParams.m_recordVotingPoints) { +#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98) + votingPoints[i].insert( + votingPoints[i].end(), + std::make_move_iterator(votingPoints[j].begin()), + std::make_move_iterator(votingPoints[j].end()) + ); +#else + votingPoints[i].insert( + votingPoints[i].end(), + votingPoints[j].begin(), + votingPoints[j].end() + ); +#endif + votingPoints.pop_back(); + } + circleCandidates.pop_back(); + circleCandidatesVotes.pop_back(); + circleCandidatesProba.pop_back(); + --nbCandidates; + // We do not update j because the new j-th candidate has not been evaluated yet + } + else { + // We will evaluate the next candidate + ++j; + } + } + // // Add the circle candidate CiC_i, potentially merged with other circle candidates, to the final list of detected circles + circleCandidates[i] = cic_i; + if (hasPerformedMerge && m_algoParams.m_recordVotingPoints) { + // Remove duplicated points + std::sort(votingPoints[i].begin(), votingPoints[i].end()); + votingPoints[i].erase(std::unique(votingPoints[i].begin(), votingPoints[i].end()), votingPoints[i].end()); + } + ++i; + } +} + +END_VISP_NAMESPACE diff --git a/modules/imgproc/src/vpCircleHoughTransform_common.cpp b/modules/imgproc/src/vpCircleHoughTransform_common.cpp new file mode 100644 index 0000000000..11114df4e0 --- /dev/null +++ b/modules/imgproc/src/vpCircleHoughTransform_common.cpp @@ -0,0 +1,419 @@ +/* + * ViSP, open source Visual Servoing Platform software. + * Copyright (C) 2005 - 2024 by Inria. All rights reserved. + * + * This software is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * See the file LICENSE.txt at the root directory of this source + * distribution for additional information about the GNU GPL. + * + * For using ViSP with software that can not be combined with the GNU + * GPL, please contact Inria about acquiring a ViSP Professional + * Edition License. + * + * See https://visp.inria.fr for more information. + * + * This software was developed at: + * Inria Rennes - Bretagne Atlantique + * Campus Universitaire de Beaulieu + * 35042 Rennes Cedex + * France + * + * If you have questions regarding the use of this file, please contact + * Inria at visp@inria.fr + * + * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE + * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include +#include + +#include + +BEGIN_VISP_NAMESPACE + +#if (VISP_CXX_STANDARD == VISP_CXX_STANDARD_98) +namespace +{ + +// Sorting by decreasing probabilities +bool hasBetterProba(std::pair a, std::pair b) +{ + return (a.second > b.second); +} + + +void +scaleFilter(vpArray2D &filter, const float &scale) +{ + unsigned int nbRows = filter.getRows(); + unsigned int nbCols = filter.getCols(); + for (unsigned int r = 0; r < nbRows; ++r) { + for (unsigned int c = 0; c < nbCols; ++c) { + filter[r][c] = filter[r][c] * scale; + } + } +} +} +#endif + +vpCircleHoughTransform::vpCircleHoughTransform() + : m_algoParams() + , mp_mask(nullptr) +{ + initGaussianFilters(); + initGradientFilters(); +} + +vpCircleHoughTransform::vpCircleHoughTransform(const vpCircleHoughTransformParams &algoParams) + : m_algoParams(algoParams) + , mp_mask(nullptr) +{ + initGaussianFilters(); + initGradientFilters(); +} + +void +vpCircleHoughTransform::init(const vpCircleHoughTransformParams &algoParams) +{ + m_algoParams = algoParams; + initGaussianFilters(); + initGradientFilters(); +} + +vpCircleHoughTransform::~vpCircleHoughTransform() +{ } + +#ifdef VISP_HAVE_NLOHMANN_JSON +using json = nlohmann::json; + +vpCircleHoughTransform::vpCircleHoughTransform(const std::string &jsonPath) +{ + initFromJSON(jsonPath); +} + +void +vpCircleHoughTransform::initFromJSON(const std::string &jsonPath) +{ + std::ifstream file(jsonPath); + if (!file.good()) { + std::stringstream ss; + ss << "Problem opening file " << jsonPath << ". Make sure it exists and is readable" << std::endl; + throw vpException(vpException::ioError, ss.str()); + } + json j; + try { + j = json::parse(file); + } + catch (json::parse_error &e) { + std::stringstream msg; + msg << "Could not parse JSON file : \n"; + + msg << e.what() << std::endl; + msg << "Byte position of error: " << e.byte; + throw vpException(vpException::ioError, msg.str()); + } + m_algoParams = j; // Call from_json(const json& j, vpDetectorDNN& *this) to read json + file.close(); + initGaussianFilters(); + initGradientFilters(); +} + +void +vpCircleHoughTransform::saveConfigurationInJSON(const std::string &jsonPath) const +{ + m_algoParams.saveConfigurationInJSON(jsonPath); +} +#endif + +void +vpCircleHoughTransform::initGaussianFilters() +{ + const int filterHalfSize = (m_algoParams.m_gaussianKernelSize + 1) / 2; + m_fg.resize(1, filterHalfSize); + vpImageFilter::getGaussianKernel(m_fg.data, m_algoParams.m_gaussianKernelSize, m_algoParams.m_gaussianStdev, true); + m_cannyVisp.setGaussianFilterParameters(m_algoParams.m_gaussianKernelSize, m_algoParams.m_gaussianStdev); +} + +void +vpCircleHoughTransform::initGradientFilters() +{ +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) // Check if cxx11 or higher + // Helper to apply the scale to the raw values of the filters + auto scaleFilter = [](vpArray2D &filter, const float &scale) { + const unsigned int nbRows = filter.getRows(); + const unsigned int nbCols = filter.getCols(); + for (unsigned int r = 0; r < nbRows; ++r) { + for (unsigned int c = 0; c < nbCols; ++c) { + filter[r][c] = filter[r][c] * scale; + } + } + }; +#endif + + const int moduloCheckForOddity = 2; + if ((m_algoParams.m_gradientFilterKernelSize % moduloCheckForOddity) != 1) { + throw vpException(vpException::badValue, "Gradient filters Kernel size should be odd."); + } + m_gradientFilterX.resize(m_algoParams.m_gradientFilterKernelSize, m_algoParams.m_gradientFilterKernelSize); + m_gradientFilterY.resize(m_algoParams.m_gradientFilterKernelSize, m_algoParams.m_gradientFilterKernelSize); + m_cannyVisp.setGradientFilterAperture(m_algoParams.m_gradientFilterKernelSize); + + float scaleX = 1.f; + float scaleY = 1.f; + unsigned int filterHalfSize = (m_algoParams.m_gradientFilterKernelSize - 1) / 2; + + if (m_algoParams.m_filteringAndGradientType == vpImageFilter::CANNY_GBLUR_SOBEL_FILTERING) { + // Compute the Sobel filters + scaleX = vpImageFilter::getSobelKernelX(m_gradientFilterX.data, filterHalfSize); + scaleY = vpImageFilter::getSobelKernelY(m_gradientFilterY.data, filterHalfSize); + } + else if (m_algoParams.m_filteringAndGradientType == vpImageFilter::CANNY_GBLUR_SCHARR_FILTERING) { + // Compute the Scharr filters + scaleX = vpImageFilter::getScharrKernelX(m_gradientFilterX.data, filterHalfSize); + scaleY = vpImageFilter::getScharrKernelY(m_gradientFilterY.data, filterHalfSize); + } + else { + std::string errMsg = "[vpCircleHoughTransform::initGradientFilters] Error: gradient filtering method \""; + errMsg += vpImageFilter::vpCannyFiltAndGradTypeToStr(m_algoParams.m_filteringAndGradientType); + errMsg += "\" has not been implemented yet\n"; + throw vpException(vpException::notImplementedError, errMsg); + } + scaleFilter(m_gradientFilterX, scaleX); + scaleFilter(m_gradientFilterY, scaleY); +} + +std::vector +vpCircleHoughTransform::detect(const vpImage &I) +{ + vpImage I_gray; + vpImageConvert::convert(I, I_gray); + return detect(I_gray); +} + +#ifdef HAVE_OPENCV_CORE +std::vector +vpCircleHoughTransform::detect(const cv::Mat &cv_I) +{ + vpImage I_gray; + vpImageConvert::convert(cv_I, I_gray); + return detect(I_gray); +} +#endif + +std::vector +vpCircleHoughTransform::detect(const vpImage &I, const int &nbCircles) +{ + std::vector detections = detect(I); + size_t nbDetections = detections.size(); + + // Prepare vector of tuple to sort by decreasing probabilities + std::vector > v_id_proba; + for (size_t i = 0; i < nbDetections; ++i) { + std::pair id_proba(i, m_finalCirclesProbabilities[i]); + v_id_proba.push_back(id_proba); + } + +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) + // Sorting by decreasing probabilities + auto hasBetterProba + = [](std::pair a, std::pair b) { + return (a.second > b.second); + }; +#endif + std::sort(v_id_proba.begin(), v_id_proba.end(), hasBetterProba); + + // Clearing the storages containing the detection results + // to have it sorted by decreasing probabilities + size_t limitMin; + if (nbCircles < 0) { + limitMin = nbDetections; + } + else { + limitMin = std::min(nbDetections, static_cast(nbCircles)); + } + + std::vector bestCircles; + std::vector copyFinalCircles = m_finalCircles; + std::vector copyFinalCirclesVotes = m_finalCircleVotes; + std::vector copyFinalCirclesProbas = m_finalCirclesProbabilities; + std::vector > > copyFinalCirclesVotingPoints = m_finalCirclesVotingPoints; + for (size_t i = 0; i < nbDetections; ++i) { + size_t id = v_id_proba[i].first; + m_finalCircles[i] = copyFinalCircles[id]; + m_finalCircleVotes[i] = copyFinalCirclesVotes[id]; + m_finalCirclesProbabilities[i] = copyFinalCirclesProbas[id]; + if (m_algoParams.m_recordVotingPoints) { + m_finalCirclesVotingPoints[i] = copyFinalCirclesVotingPoints[id]; + } + if (i < limitMin) { + bestCircles.push_back(m_finalCircles[i]); + } + } + + return bestCircles; +} + +std::vector +vpCircleHoughTransform::detect(const vpImage &I) +{ + // Cleaning results of potential previous detection + m_centerCandidatesList.clear(); + m_centerVotes.clear(); + m_edgePointsList.clear(); + m_circleCandidates.clear(); + m_circleCandidatesVotes.clear(); + m_circleCandidatesProbabilities.clear(); + m_finalCircles.clear(); + m_finalCircleVotes.clear(); + + // Ensuring that the difference between the max and min radii is big enough to take into account + // the pixelization of the image + const float minRadiusDiff = 3.f; + if ((m_algoParams.m_maxRadius - m_algoParams.m_minRadius) < minRadiusDiff) { + if (m_algoParams.m_minRadius > (minRadiusDiff / 2.f)) { + m_algoParams.m_maxRadius += minRadiusDiff / 2.f; + m_algoParams.m_minRadius -= minRadiusDiff / 2.f; + } + else { + m_algoParams.m_maxRadius += minRadiusDiff - m_algoParams.m_minRadius; + m_algoParams.m_minRadius = 0.f; + } + } + + // Ensuring that the difference between the max and min center position is big enough to take into account + // the pixelization of the image + const float minCenterPositionDiff = 3.f; + if ((m_algoParams.m_centerXlimits.second - m_algoParams.m_centerXlimits.first) < minCenterPositionDiff) { + m_algoParams.m_centerXlimits.second += static_cast(minCenterPositionDiff / 2.f); + m_algoParams.m_centerXlimits.first -= static_cast(minCenterPositionDiff / 2.f); + } + if ((m_algoParams.m_centerYlimits.second - m_algoParams.m_centerYlimits.first) < minCenterPositionDiff) { + m_algoParams.m_centerYlimits.second += static_cast(minCenterPositionDiff / 2.f); + m_algoParams.m_centerYlimits.first -= static_cast(minCenterPositionDiff / 2.f); + } + + // First thing, we need to apply a Gaussian filter on the image to remove some spurious noise + // Then, we need to compute the image gradients in order to be able to perform edge detection + computeGradients(I); + + // Using the gradients, it is now possible to perform edge detection + // We rely on the Canny edge detector + // It will also give us the connected edged points + edgeDetection(I); + + // From the edge map and gradient information, it is possible to compute + // the center point candidates + computeCenterCandidates(); + + // From the edge map and center point candidates, we can compute candidate + // circles. These candidate circles are circles whose center belong to + // the center point candidates and whose radius is a "radius bin" that got + // enough votes by computing the distance between each point of the edge map + // and the center point candidate + computeCircleCandidates(); + + // Finally, we perform a merging operation that permits to merge circles + // respecting similarity criteria (distance between centers and similar radius) + mergeCircleCandidates(); + + return m_finalCircles; +} + +bool +operator==(const vpImageCircle &a, const vpImageCircle &b) +{ + vpImagePoint aCenter = a.getCenter(); + vpImagePoint bCenter = b.getCenter(); + bool haveSameCenter = (std::abs(aCenter.get_u() - bCenter.get_u()) + + std::abs(aCenter.get_v() - bCenter.get_v())) <= (2. * std::numeric_limits::epsilon()); + bool haveSameRadius = std::abs(a.getRadius() - b.getRadius()) <= (2.f * std::numeric_limits::epsilon()); + return (haveSameCenter && haveSameRadius); +} + +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) +void vpCircleHoughTransform::computeVotingMask(const vpImage &I, const std::vector &detections, + std::optional< vpImage > &mask, std::optional>>> &opt_votingPoints) const +#else +void vpCircleHoughTransform::computeVotingMask(const vpImage &I, const std::vector &detections, + vpImage **mask, std::vector > > **opt_votingPoints) const +#endif +{ + if (!m_algoParams.m_recordVotingPoints) { + // We weren't asked to remember the voting points +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) + mask = std::nullopt; + opt_votingPoints = std::nullopt; +#else + *mask = nullptr; + *opt_votingPoints = nullptr; +#endif + return; + } + +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) + mask = vpImage(I.getHeight(), I.getWidth(), false); + opt_votingPoints = std::vector>>(); +#else + *mask = new vpImage(I.getHeight(), I.getWidth(), false); + *opt_votingPoints = new std::vector > >(); +#endif + +#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98) + for (const auto &detection : detections) +#else + const size_t nbDetections = detections.size(); + for (size_t i = 0; i < nbDetections; ++i) +#endif + { + bool hasFoundSimilarCircle = false; + unsigned int nbPreviouslyDetected = static_cast(m_finalCircles.size()); + unsigned int id = 0; + // Looking for a circle that was detected and is similar to the one given to the function + while ((id < nbPreviouslyDetected) && (!hasFoundSimilarCircle)) { + vpImageCircle previouslyDetected = m_finalCircles[id]; +#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98) + if (previouslyDetected == detection) +#else + if (previouslyDetected == detections[i]) +#endif + { + hasFoundSimilarCircle = true; + // We found a circle that is similar to the one given to the function => updating the mask + const unsigned int nbVotingPoints = static_cast(m_finalCirclesVotingPoints[id].size()); + for (unsigned int idPoint = 0; idPoint < nbVotingPoints; ++idPoint) { + const std::pair &votingPoint = m_finalCirclesVotingPoints[id][idPoint]; +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) + (*mask)[votingPoint.first][votingPoint.second] = true; +#else + (**mask)[votingPoint.first][votingPoint.second] = true; +#endif + } +#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) + opt_votingPoints->push_back(m_finalCirclesVotingPoints[id]); +#else + (**opt_votingPoints).push_back(m_finalCirclesVotingPoints[id]); +#endif + } + ++id; + } + } +} + +std::string +vpCircleHoughTransform::toString() const +{ + return m_algoParams.toString(); +} + +std::ostream & +operator<<(std::ostream &os, const vpCircleHoughTransform &detector) +{ + os << detector.toString(); + return os; +} + +END_VISP_NAMESPACE diff --git a/modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.cpp b/modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.cpp index fc9257adde..9b42523239 100644 --- a/modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.cpp +++ b/modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.cpp @@ -810,11 +810,14 @@ bool lmderMostInnerLoop(ComputeFunctionAndJacobian ptr_fcn, int m, int n, } if (delta <= (xtol * xnorm)) { - *info = 2; + const int flagInfo = 2; + *info = flagInfo; } - if ((std::fabs(actred) <= ftol) && (prered <= ftol) && ((tol5 * ratio) <= 1.0) && (*info == 2)) { - *info = 3; + const int info2 = 2; + if ((std::fabs(actred) <= ftol) && (prered <= ftol) && ((tol5 * ratio) <= 1.0) && (*info == info2)) { + const int flagInfo = 3; + *info = flagInfo; } if (*info != 0) { @@ -839,19 +842,23 @@ bool lmderMostInnerLoop(ComputeFunctionAndJacobian ptr_fcn, int m, int n, */ if (*nfev >= maxfev) { - *info = 5; + const int flagInfo = 5; + *info = flagInfo; } if ((std::fabs(actred) <= epsmch) && (prered <= epsmch) && ((tol5 * ratio) <= 1.0)) { - *info = 6; + const int flagInfo = 6; + *info = flagInfo; } if (delta <= (epsmch * xnorm)) { - *info = 7; + const int flagInfo = 7; + *info = flagInfo; } if (gnorm <= epsmch) { - *info = 8; + const int flagInfo = 8; + *info = flagInfo; } if (*info != 0) { @@ -934,7 +941,8 @@ int lmder(ComputeFunctionAndJacobian ptr_fcn, int m, int n, return count; } - if (mode == 2) { + const int mode2 = 2; + if (mode == mode2) { for (j = 0; j < n; ++j) { if (diag[j] <= 0.0) { /* @@ -991,6 +999,7 @@ int lmder(ComputeFunctionAndJacobian ptr_fcn, int m, int n, par = 0.0; iter = 1; + const int iflag2 = 2; /* * debut de la boucle la plus externe. @@ -1001,7 +1010,7 @@ int lmder(ComputeFunctionAndJacobian ptr_fcn, int m, int n, * calcul de la matrice jacobienne. */ - iflag = 2; + iflag = iflag2; (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag); @@ -1062,7 +1071,7 @@ int lmder(ComputeFunctionAndJacobian ptr_fcn, int m, int n, */ if (iter == 1) { - if (mode != 2) { + if (mode != mode2) { for (j = 0; j < n; ++j) { diag[j] = wa2[j]; if (std::fabs(wa2[j]) <= std::numeric_limits::epsilon()) { @@ -1142,7 +1151,8 @@ int lmder(ComputeFunctionAndJacobian ptr_fcn, int m, int n, */ if (gnorm <= gtol) { - *info = 4; + const int info4 = 4; + *info = info4; } if (*info != 0) { @@ -1166,7 +1176,7 @@ int lmder(ComputeFunctionAndJacobian ptr_fcn, int m, int n, * remise a l'echelle si necessaire. */ - if (mode != 2) { + if (mode != mode2) { for (j = 0; j < n; ++j) { diag[j] = vpMath::maximum(diag[j], wa2[j]); } @@ -1194,26 +1204,28 @@ int lmder1(ComputeFunctionAndJacobian ptr_fcn, int m, int n, *info = 0; /* verification des parametres en entree qui causent des erreurs */ - - if (/*(n <= 0) ||*/ (m < n) || (ldfjac < m) || (tol < 0.0) || (lwa < ((5 * n) + m))) { - printf("%d %d %d %d \n", (m < n), (ldfjac < m), (tol < 0.0), (lwa < ((5 * n) + m))); + const int lwaThresh = ((5 * n) + m); + if (/*(n <= 0) ||*/ (m < n) || (ldfjac < m) || (tol < 0.0) || (lwa < lwaThresh)) { + printf("%d %d %d %d \n", (m < n), (ldfjac < m), (tol < 0.0), (lwa < lwaThresh)); return (-1); } /* appel a lmder */ - - maxfev = static_cast(100 * (n + 1)); + const int factor100 = 100; + maxfev = static_cast(factor100 * (n + 1)); ftol = tol; xtol = tol; gtol = 0.0; mode = 1; nprint = 0; + const int factor2 = 2, factor3 = 3, factor4 = 4, factor5 = 5; lmder(ptr_fcn, m, n, x, fvec, fjac, ldfjac, ftol, xtol, gtol, maxfev, wa, mode, factor, nprint, info, &nfev, &njev, - ipvt, &wa[n], &wa[2 * n], &wa[3 * n], &wa[4 * n], &wa[5 * n]); + ipvt, &wa[n], &wa[factor2 * n], &wa[factor3 * n], &wa[factor4 * n], &wa[factor5 * n]); - if (*info == 8) { - *info = 4; + const int info8 = 8, info4 = 4; + if (*info == info8) { + *info = info4; } return 0; diff --git a/modules/vision/src/pose-estimation/vpPose.cpp b/modules/vision/src/pose-estimation/vpPose.cpp index 781d992239..d237d63869 100644 --- a/modules/vision/src/pose-estimation/vpPose.cpp +++ b/modules/vision/src/pose-estimation/vpPose.cpp @@ -206,13 +206,14 @@ bool vpPose::coplanar(int &coplanar_plane_type, double *p_a, double *p_b, double z2 = P2.get_oZ(); z3 = P3.get_oZ(); + const int idX = 0, idY = 1, idZ = 2; vpColVector a_b(3), b_c(3), cross_prod; - a_b[0] = x1 - x2; - a_b[1] = y1 - y2; - a_b[2] = z1 - z2; - b_c[0] = x2 - x3; - b_c[1] = y2 - y3; - b_c[2] = z2 - z3; + a_b[idX] = x1 - x2; + a_b[idY] = y1 - y2; + a_b[idZ] = z1 - z2; + b_c[idX] = x2 - x3; + b_c[idY] = y2 - y3; + b_c[idZ] = z2 - z3; cross_prod = vpColVector::crossProd(a_b, b_c); if (cross_prod.sumSquare() <= std::numeric_limits::epsilon()) { diff --git a/modules/vision/src/pose-estimation/vpPoseDementhon.cpp b/modules/vision/src/pose-estimation/vpPoseDementhon.cpp index 41349a9b98..feee63caea 100644 --- a/modules/vision/src/pose-estimation/vpPoseDementhon.cpp +++ b/modules/vision/src/pose-estimation/vpPoseDementhon.cpp @@ -198,9 +198,9 @@ void vpPose::poseDementhonNonPlan(vpHomogeneousMatrix &cMo) ++cpt; } // go back to the initial frame - cMo[idX][3] -= ((cdg[idX] * cMo[idX][idX]) + (cdg[idY] * cMo[idX][idY]) + (cdg[idZ] * cMo[idX][idZ])); - cMo[idY][3] -= ((cdg[idX] * cMo[idY][idX]) + (cdg[idY] * cMo[idY][idY]) + (cdg[idZ] * cMo[idY][idZ])); - cMo[idZ][3] -= ((cdg[idX] * cMo[idZ][idX]) + (cdg[idY] * cMo[idZ][idY]) + (cdg[idZ] * cMo[idZ][idZ])); + cMo[idX][idTranslation] -= ((cdg[idX] * cMo[idX][idX]) + (cdg[idY] * cMo[idX][idY]) + (cdg[idZ] * cMo[idX][idZ])); + cMo[idY][idTranslation] -= ((cdg[idX] * cMo[idY][idX]) + (cdg[idY] * cMo[idY][idY]) + (cdg[idZ] * cMo[idY][idZ])); + cMo[idZ][idTranslation] -= ((cdg[idX] * cMo[idZ][idX]) + (cdg[idY] * cMo[idZ][idY]) + (cdg[idZ] * cMo[idZ][idZ])); } static void calculRTheta(double s, double c, double &r, double &theta) @@ -385,9 +385,9 @@ void vpPose::poseDementhonPlan(vpHomogeneousMatrix &cMo) std::list::const_iterator listp_end_decl2 = listP.end(); for (std::list::const_iterator it = listP.begin(); it != listp_end_decl2; ++it) { P = (*it); - P.set_oX(P.get_oX() - cdg[0]); - P.set_oY(P.get_oY() - cdg[1]); - P.set_oZ(P.get_oZ() - cdg[2]); + P.set_oX(P.get_oX() - cdg[idX]); + P.set_oY(P.get_oY() - cdg[idY]); + P.set_oZ(P.get_oZ() - cdg[idZ]); c3d.push_back(P); } diff --git a/modules/vision/src/pose-estimation/vpPoseLagrange.cpp b/modules/vision/src/pose-estimation/vpPoseLagrange.cpp index c8885c9bf6..2ef27c04e4 100644 --- a/modules/vision/src/pose-estimation/vpPoseLagrange.cpp +++ b/modules/vision/src/pose-estimation/vpPoseLagrange.cpp @@ -259,13 +259,14 @@ void vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, bool *p_isPlan, double * unsigned int k = 0; unsigned int nl = npt * 2; - const unsigned int nbColsA = 3, nbColsB = 6; + const unsigned int nbColsA = 3U, nbColsB = 6U; vpMatrix A(nl, nbColsA); vpMatrix B(nl, nbColsB); vpPoint P; std::list::const_iterator listp_end = listP.end(); - const unsigned int idHomogeneous = 3, sizeHomogeneous = 4; + const unsigned int idHomogeneous = 3U, sizeHomogeneous = 4U; + const unsigned int offsetk = 2U; for (std::list::const_iterator it = listP.begin(); it != listp_end; ++it) { P = *it; @@ -299,7 +300,7 @@ void vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, bool *p_isPlan, double * B[k + 1][index_4] = -1.0; B[k + 1][index_5] = P.get_y(); - k += 2; + k += offsetk; } const unsigned int sizeX1 = nbColsA, sizeX2 = nbColsB, lastX2 = sizeX2 - 1; // X1 is of the size of A^T A and X2 of B^T B vpColVector X1(sizeX1); @@ -349,7 +350,7 @@ void vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, bool *p_isPlan, double * for (unsigned int i = 0; i < val_3; ++i) { cMf[i][index_0] = X1[i]; cMf[i][index_1] = X2[i]; - cMf[i][index_3] = X2[i + 3]; + cMf[i][index_3] = X2[i + index_3]; } // Apply the transform to go back to object frame @@ -383,6 +384,7 @@ void vpPose::poseLagrangeNonPlan(vpHomogeneousMatrix &cMo) const unsigned int id0 = 0, id1 = 1, id2 = 2; const unsigned int id3 = 3, id4 = 4, id5 = 5; const unsigned int id6 = 6, id7 = 7, id8 = 8; + const unsigned int offsetk = 2U; for (std::list::const_iterator it = listP.begin(); it != listp_end; ++it) { P = *it; a[k][id0] = -P.get_oX(); @@ -417,7 +419,7 @@ void vpPose::poseLagrangeNonPlan(vpHomogeneousMatrix &cMo) b[k + 1][id7] = -1.0; b[k + 1][id8] = P.get_y(); - k += 2; + k += offsetk; } vpColVector X1(nbColsA); // X1 is of size A^T A vpColVector X2(nbColsB); // X2 is of size B^T B @@ -444,7 +446,8 @@ void vpPose::poseLagrangeNonPlan(vpHomogeneousMatrix &cMo) } s = 1.0 / sqrt(s); - for (i = 0; i < 3; ++i) { + const unsigned int istop = 3; + for (i = 0; i < istop; ++i) { X2[i] *= s; } /* X2^T X2 = 1 */ @@ -455,7 +458,7 @@ void vpPose::poseLagrangeNonPlan(vpHomogeneousMatrix &cMo) const unsigned int nc1 = 3, nc3 = 6; calculTranslation(a, b, nl, nc1, nc3, X1, X2); - for (i = 0; i < 3; ++i) { + for (i = 0; i < id3; ++i) { cMo[i][id0] = X1[i]; cMo[i][id1] = X2[i]; cMo[i][id2] = X2[i + id3]; diff --git a/modules/vision/src/pose-estimation/vpPoseRGBD.cpp b/modules/vision/src/pose-estimation/vpPoseRGBD.cpp index 651de31321..47be6fae92 100644 --- a/modules/vision/src/pose-estimation/vpPoseRGBD.cpp +++ b/modules/vision/src/pose-estimation/vpPoseRGBD.cpp @@ -202,7 +202,8 @@ bool vpPose::computePlanarObjectPoseFromRGBD(const vpImage &depthMap, con unsigned int nb_points_3d = static_cast(points_3d.size() / 3); - if (nb_points_3d > 4) { + const unsigned int minNbPoints = 4; + if (nb_points_3d > minNbPoints) { std::vector p, q; // Plane equation diff --git a/modules/vision/src/pose-estimation/vpPoseRansac.cpp b/modules/vision/src/pose-estimation/vpPoseRansac.cpp index 9d1e11a157..eefbe8cb92 100644 --- a/modules/vision/src/pose-estimation/vpPoseRansac.cpp +++ b/modules/vision/src/pose-estimation/vpPoseRansac.cpp @@ -68,16 +68,17 @@ struct CompareObjectPointDegenerate const unsigned int index_0 = 0; const unsigned int index_1 = 1; const unsigned int index_2 = 2; + const double val3 = 3.; bool rc = false; const double dist1 = (point1.get_oX() * point1.get_oX()) + (point1.get_oY() * point1.get_oY()) + (point1.get_oZ() * point1.get_oZ()); const double dist2 = (point2.get_oX() * point2.get_oX()) + (point2.get_oY() * point2.get_oY()) + (point2.get_oZ() * point2.get_oZ()); - if ((dist1 - dist2) < (-3 * EPS * EPS)) { + if ((dist1 - dist2) < (-val3 * EPS * EPS)) { return true; } - if ((dist1 - dist2) > (3 * EPS * EPS)) { + if ((dist1 - dist2) > (val3 * EPS * EPS)) { return false; } @@ -114,10 +115,11 @@ struct CompareImagePointDegenerate bool rc = false; const double dist1 = (point1.get_x() * point1.get_x()) + (point1.get_y() * point1.get_y()); const double dist2 = (point2.get_x() * point2.get_x()) + (point2.get_y() * point2.get_y()); - if ((dist1 - dist2) < (-2 * EPS * EPS)) { + const double val2 = 2.; + if ((dist1 - dist2) < (-val2 * EPS * EPS)) { return true; } - if ((dist1 - dist2) > (2 * EPS * EPS)) { + if ((dist1 - dist2) > (val2 * EPS * EPS)) { return false; } @@ -230,7 +232,7 @@ bool vpPose::vpRansacFunctor::poseRansacImpl() bool stop_for_loop = false; if (poseMin.npt < nbMinRandom) { ++nbTrials; - stop_for_loop = true;; + stop_for_loop = true; } if (!stop_for_loop) {