From 7254ec5ca807d0387fa0398e2c6607c3afb231fc Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Tue, 9 Apr 2024 18:59:40 +0200 Subject: [PATCH] Satisfy quality rules --- modules/core/src/image/vpImageConvert.cpp | 74 ++--- modules/core/src/image/vpImageConvert_yuv.cpp | 272 ++++++++++++------ modules/core/src/tools/file/vpIoTools.cpp | 172 ++++++----- .../homography-estimation/vpHomography.cpp | 8 +- .../vpHomographyRansac.cpp | 103 +++++-- .../homography-estimation/vpHomographyVVS.cpp | 110 ++++--- 6 files changed, 460 insertions(+), 279 deletions(-) diff --git a/modules/core/src/image/vpImageConvert.cpp b/modules/core/src/image/vpImageConvert.cpp index 83db7d22ff..6913994fdb 100644 --- a/modules/core/src/image/vpImageConvert.cpp +++ b/modules/core/src/image/vpImageConvert.cpp @@ -1595,10 +1595,10 @@ void vpImageConvert::computeYCbCrLUT() while (index--) { int aux = index - 128; - vpImageConvert::vpCrr[index] = static_cast(364.6610 * aux) >> 8; - vpImageConvert::vpCgb[index] = static_cast(-89.8779 * aux) >> 8; - vpImageConvert::vpCgr[index] = static_cast(-185.8154 * aux) >> 8; - vpImageConvert::vpCbb[index] = static_cast(460.5724 * aux) >> 8; + vpImageConvert::vpCrr[index] = static_cast((364.6610 * aux) / 256); + vpImageConvert::vpCgb[index] = static_cast((-89.8779 * aux) / 256); + vpImageConvert::vpCgr[index] = static_cast((-185.8154 * aux) / 256); + vpImageConvert::vpCbb[index] = static_cast((460.5724 * aux) / 256); } YCbCrLUTcomputed = true; @@ -1641,7 +1641,7 @@ void vpImageConvert::YCbCrToRGB(unsigned char *ycbcr, unsigned char *rgb, unsign while (size--) { int val_r, val_g, val_b; - if (!(col++ % 2)) { + if (!(col % 2)) { cbv = pt_ycbcr + 1; crv = pt_ycbcr + 3; } @@ -1652,11 +1652,12 @@ void vpImageConvert::YCbCrToRGB(unsigned char *ycbcr, unsigned char *rgb, unsign vpDEBUG_TRACE(5, "[%d] R: %d G: %d B: %d\n", size, val_r, val_g, val_b); - *pt_rgb++ = (val_r < 0) ? 0u : ((val_r > 255) ? 255u : (unsigned char)val_r); // Red component. - *pt_rgb++ = (val_g < 0) ? 0u : ((val_g > 255) ? 255u : (unsigned char)val_g); // Green component. - *pt_rgb++ = (val_b < 0) ? 0u : ((val_b > 255) ? 255u : (unsigned char)val_b); // Blue component. + *pt_rgb++ = (val_r < 0) ? 0u : ((val_r > 255) ? 255u : static_cast(val_r)); // Red component. + *pt_rgb++ = (val_g < 0) ? 0u : ((val_g > 255) ? 255u : static_cast(val_g)); // Green component. + *pt_rgb++ = (val_b < 0) ? 0u : ((val_b > 255) ? 255u : static_cast(val_b)); // Blue component. pt_ycbcr += 2; + ++col; } } @@ -1700,7 +1701,7 @@ void vpImageConvert::YCbCrToRGBa(unsigned char *ycbcr, unsigned char *rgba, unsi while (size--) { int val_r, val_g, val_b; - if (!(col++ % 2)) { + if (!(col % 2)) { cbv = pt_ycbcr + 1; crv = pt_ycbcr + 3; } @@ -1711,12 +1712,13 @@ void vpImageConvert::YCbCrToRGBa(unsigned char *ycbcr, unsigned char *rgba, unsi vpDEBUG_TRACE(5, "[%d] R: %d G: %d B: %d\n", size, val_r, val_g, val_b); - *pt_rgba++ = (val_r < 0) ? 0u : ((val_r > 255) ? 255u : (unsigned char)val_r); // Red component. - *pt_rgba++ = (val_g < 0) ? 0u : ((val_g > 255) ? 255u : (unsigned char)val_g); // Green component. - *pt_rgba++ = (val_b < 0) ? 0u : ((val_b > 255) ? 255u : (unsigned char)val_b); // Blue component. + *pt_rgba++ = (val_r < 0) ? 0u : ((val_r > 255) ? 255u : static_cast(val_r)); // Red component. + *pt_rgba++ = (val_g < 0) ? 0u : ((val_g > 255) ? 255u : static_cast(val_g)); // Green component. + *pt_rgba++ = (val_b < 0) ? 0u : ((val_b > 255) ? 255u : static_cast(val_b)); // Blue component. *pt_rgba++ = vpRGBa::alpha_default; pt_ycbcr += 2; + ++col; } } @@ -1786,7 +1788,7 @@ void vpImageConvert::YCrCbToRGB(unsigned char *ycrcb, unsigned char *rgb, unsign while (size--) { int val_r, val_g, val_b; - if (!(col++ % 2)) { + if (!(col % 2)) { crv = pt_ycbcr + 1; cbv = pt_ycbcr + 3; } @@ -1797,11 +1799,12 @@ void vpImageConvert::YCrCbToRGB(unsigned char *ycrcb, unsigned char *rgb, unsign vpDEBUG_TRACE(5, "[%d] R: %d G: %d B: %d\n", size, val_r, val_g, val_b); - *pt_rgb++ = (val_r < 0) ? 0u : ((val_r > 255) ? 255u : (unsigned char)val_r); // Red component. - *pt_rgb++ = (val_g < 0) ? 0u : ((val_g > 255) ? 255u : (unsigned char)val_g); // Green component. - *pt_rgb++ = (val_b < 0) ? 0u : ((val_b > 255) ? 255u : (unsigned char)val_b); // Blue component. + *pt_rgb++ = (val_r < 0) ? 0u : ((val_r > 255) ? 255u : static_cast(val_r)); // Red component. + *pt_rgb++ = (val_g < 0) ? 0u : ((val_g > 255) ? 255u : static_cast(val_g)); // Green component. + *pt_rgb++ = (val_b < 0) ? 0u : ((val_b > 255) ? 255u : static_cast(val_b)); // Blue component. pt_ycbcr += 2; + ++col; } } @@ -1844,7 +1847,7 @@ void vpImageConvert::YCrCbToRGBa(unsigned char *ycrcb, unsigned char *rgba, unsi while (size--) { int val_r, val_g, val_b; - if (!(col++ % 2)) { + if (!(col % 2)) { crv = pt_ycbcr + 1; cbv = pt_ycbcr + 3; } @@ -1855,12 +1858,13 @@ void vpImageConvert::YCrCbToRGBa(unsigned char *ycrcb, unsigned char *rgba, unsi vpDEBUG_TRACE(5, "[%d] R: %d G: %d B: %d\n", size, val_r, val_g, val_b); - *pt_rgba++ = (val_r < 0) ? 0u : ((val_r > 255) ? 255u : (unsigned char)val_r); // Red component. - *pt_rgba++ = (val_g < 0) ? 0u : ((val_g > 255) ? 255u : (unsigned char)val_g); // Green component. - *pt_rgba++ = (val_b < 0) ? 0u : ((val_b > 255) ? 255u : (unsigned char)val_b); // Blue component. + *pt_rgba++ = (val_r < 0) ? 0u : ((val_r > 255) ? 255u : static_cast(val_r)); // Red component. + *pt_rgba++ = (val_g < 0) ? 0u : ((val_g > 255) ? 255u : static_cast(val_g)); // Green component. + *pt_rgba++ = (val_b < 0) ? 0u : ((val_b > 255) ? 255u : static_cast(val_b)); // Blue component. *pt_rgba++ = vpRGBa::alpha_default; pt_ycbcr += 2; + ++col; } } @@ -2018,23 +2022,23 @@ void vpImageConvert::merge(const vpImage *R, const vpImage mapOfWidths, mapOfHeights; if (R != nullptr) { - mapOfWidths[R->getWidth()]++; - mapOfHeights[R->getHeight()]++; + ++mapOfWidths[R->getWidth()]; + ++mapOfHeights[R->getHeight()]; } if (G != nullptr) { - mapOfWidths[G->getWidth()]++; - mapOfHeights[G->getHeight()]++; + ++mapOfWidths[G->getWidth()]; + ++mapOfHeights[G->getHeight()]; } if (B != nullptr) { - mapOfWidths[B->getWidth()]++; - mapOfHeights[B->getHeight()]++; + ++mapOfWidths[B->getWidth()]; + ++mapOfHeights[B->getHeight()]; } if (a != nullptr) { - mapOfWidths[a->getWidth()]++; - mapOfHeights[a->getHeight()]++; + ++mapOfWidths[a->getWidth()]; + ++mapOfHeights[a->getHeight()]; } if ((mapOfWidths.size() == 1) && (mapOfHeights.size() == 1)) { @@ -2090,12 +2094,13 @@ void vpImageConvert::merge(const vpImage *R, const vpImage(size) * 2) - 1; + int j = static_cast(size) - 1; while (i >= 0) { int y = grey16[i--]; - grey[j--] = static_cast((y + (grey16[i--] << 8)) >> 8); + grey[j--] = static_cast((y + (grey16[i] * 256)) / 256); + --i; } } @@ -2112,12 +2117,13 @@ void vpImageConvert::MONO16ToGrey(unsigned char *grey16, unsigned char *grey, un */ void vpImageConvert::MONO16ToRGBa(unsigned char *grey16, unsigned char *rgba, unsigned int size) { - int i = (((int)size) << 1) - 1; - int j = static_cast(size * 4 - 1); + int i = (static_cast(size) * 2) - 1; + int j = (static_cast(size) * 4) - 1; while (i >= 0) { int y = grey16[i--]; - unsigned char v = static_cast((y + (grey16[i--] << 8)) >> 8); + unsigned char v = static_cast((y + (grey16[i] * 256)) / 256); + --i; rgba[j--] = vpRGBa::alpha_default; rgba[j--] = v; rgba[j--] = v; diff --git a/modules/core/src/image/vpImageConvert_yuv.cpp b/modules/core/src/image/vpImageConvert_yuv.cpp index b2ac34c5ac..bec8f5a03a 100644 --- a/modules/core/src/image/vpImageConvert_yuv.cpp +++ b/modules/core/src/image/vpImageConvert_yuv.cpp @@ -41,13 +41,19 @@ #include #include -#define vpSAT(c) \ - if (c & (~255)) { \ - if (c < 0) \ - {c = 0;} \ - else \ - {c = 255;} \ +namespace +{ +void vpSAT(int &c) +{ + if (c < 0) { + c = 0; } + else { + c = 255; + } +} +}; + /*! Convert an image from YUYV 4:2:2 (y0 u01 y1 v01 y2 u23 y3 v23 ...) to RGB32. Destination rgba memory area has to be allocated before. @@ -74,12 +80,16 @@ void vpImageConvert::YUYVToRGBa(unsigned char *yuyv, unsigned char *rgba, unsign while (h--) { int c = w / 2; while (c--) { - y1 = *s++; + y1 = *s; + ++s; cb = ((*s - 128) * 454) / 256; - cg = (*s++ - 128) * 88; - y2 = *s++; + cg = (*s - 128) * 88; + ++s; + y2 = *s; + ++s; cr = ((*s - 128) * 359) / 256; - cg = (cg + ((*s++ - 128) * 183)) / 256; + cg = (cg + ((*s - 128) * 183)) / 256; + ++s; r = y1 + cr; b = y1 + cb; @@ -132,17 +142,23 @@ void vpImageConvert::YUYVToRGB(unsigned char *yuyv, unsigned char *rgb, unsigned while (h--) { int c = w / 2; while (c--) { - y1 = *s++; + y1 = *s; + ++s; cb = ((*s - 128) * 454) / 256; - cg = (*s++ - 128) * 88; - y2 = *s++; + cg = (*s - 128) * 88; + ++s; + y2 = *s; + ++s; cr = ((*s - 128) * 359) / 256; - cg = (cg + ((*s++ - 128) * 183)) / 256; + cg = (cg + ((*s - 128) * 183)) / 256; + ++s; r = y1 + cr; b = y1 + cb; g = y1 - cg; - vpSAT(r); vpSAT(g); vpSAT(b); + vpSAT(r); + vpSAT(g); + vpSAT(b); *d++ = static_cast(r); *d++ = static_cast(g); @@ -151,7 +167,9 @@ void vpImageConvert::YUYVToRGB(unsigned char *yuyv, unsigned char *rgb, unsigned r = y2 + cr; b = y2 + cb; g = y2 - cg; - vpSAT(r); vpSAT(g); vpSAT(b); + vpSAT(r); + vpSAT(g); + vpSAT(b); *d++ = static_cast(r); *d++ = static_cast(g); @@ -193,14 +211,20 @@ void vpImageConvert::YUYVToGrey(unsigned char *yuyv, unsigned char *grey, unsign void vpImageConvert::YUV411ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int size) { for (unsigned int i = size / 4; i; --i) { - int U = static_cast((*yuv++ - 128) * 0.354); + int U = static_cast((*yuv - 128) * 0.354); + ++yuv; int U5 = 5 * U; - int Y0 = *yuv++; - int Y1 = *yuv++; - int V = static_cast((*yuv++ - 128) * 0.707); + int Y0 = *yuv; + ++yuv; + int Y1 = *yuv; + ++yuv; + int V = static_cast((*yuv - 128) * 0.707); + ++yuv; int V2 = 2 * V; - int Y2 = *yuv++; - int Y3 = *yuv++; + int Y2 = *yuv; + ++yuv; + int Y3 = *yuv; + ++yuv; int UV = -U - V; // Original equations @@ -283,12 +307,16 @@ void vpImageConvert::YUV411ToRGBa(unsigned char *yuv, unsigned char *rgba, unsig void vpImageConvert::YUV422ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int size) { for (unsigned int i = size / 2; i; --i) { - int U = static_cast((*yuv++ - 128) * 0.354); + int U = static_cast((*yuv - 128) * 0.354); + ++yuv; int U5 = 5 * U; - int Y0 = *yuv++; - int V = static_cast((*yuv++ - 128) * 0.707); + int Y0 = *yuv; + ++yuv; + int V = static_cast((*yuv - 128) * 0.707); + ++yuv; int V2 = 2 * V; - int Y1 = *yuv++; + int Y1 = *yuv; + ++yuv; int UV = -U - V; //--- @@ -360,12 +388,16 @@ void vpImageConvert::YUV411ToGrey(unsigned char *yuv, unsigned char *grey, unsig void vpImageConvert::YUV422ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int size) { for (unsigned int i = size / 2; i; --i) { - int U = static_cast((*yuv++ - 128) * 0.354); + int U = static_cast((*yuv - 128) * 0.354); + ++yuv; int U5 = 5 * U; - int Y0 = *yuv++; - int V = static_cast((*yuv++ - 128) * 0.707); + int Y0 = *yuv; + ++yuv; + int V = static_cast((*yuv - 128) * 0.707); + ++yuv; int V2 = 2 * V; - int Y1 = *yuv++; + int Y1 = *yuv; + ++yuv; int UV = -U - V; //--- @@ -431,14 +463,20 @@ void vpImageConvert::YUV422ToGrey(unsigned char *yuv, unsigned char *grey, unsig void vpImageConvert::YUV411ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int size) { for (unsigned int i = size / 4; i; --i) { - int U = static_cast((*yuv++ - 128) * 0.354); + int U = static_cast((*yuv - 128) * 0.354); + ++yuv; int U5 = 5 * U; - int Y0 = *yuv++; - int Y1 = *yuv++; - int V = static_cast((*yuv++ - 128) * 0.707); + int Y0 = *yuv; + ++yuv; + int Y1 = *yuv; + ++yuv; + int V = static_cast((*yuv - 128) * 0.707); + ++yuv; int V2 = 2 * V; - int Y2 = *yuv++; - int Y3 = *yuv++; + int Y2 = *yuv; + ++yuv; + int Y3 = *yuv; + ++yuv; int UV = -U - V; // Original equations @@ -514,7 +552,6 @@ void vpImageConvert::YUV411ToRGB(unsigned char *yuv, unsigned char *rgb, unsigne */ void vpImageConvert::YUV420ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int width, unsigned int height) { - // std::cout << "call optimized ConvertYUV420ToRGBa()" << std::endl; int U, V, R, G, B, V2, U5, UV; int Y0, Y1, Y2, Y3; unsigned int size = width * height; @@ -523,15 +560,19 @@ void vpImageConvert::YUV420ToRGBa(unsigned char *yuv, unsigned char *rgba, unsig const unsigned int halfHeight = height / 2, halfWidth = width / 2; for (unsigned int i = 0; i < halfHeight; ++i) { for (unsigned int j = 0; j < halfWidth; ++j) { - U = static_cast((*iU++ - 128) * 0.354); + U = static_cast(((*iU) - 128) * 0.354); + ++iU; U5 = 5 * U; - V = static_cast((*iV++ - 128) * 0.707); + V = static_cast(((*iV) - 128) * 0.707); + ++iV; V2 = 2 * V; UV = -U - V; - Y0 = *yuv++; + Y0 = *yuv; + ++yuv; Y1 = *yuv; yuv = yuv + (width - 1); - Y2 = *yuv++; + Y2 = *yuv; + ++yuv; Y3 = *yuv; yuv = (yuv - width) + 1; @@ -567,7 +608,7 @@ void vpImageConvert::YUV420ToRGBa(unsigned char *yuv, unsigned char *rgba, unsig *rgba++ = static_cast(G); *rgba++ = static_cast(B); *rgba = vpRGBa::alpha_default; - rgba = rgba + 4 * width - 7; + rgba = (rgba + (4 * width)) - 7; //--- R = Y2 + V2; @@ -623,15 +664,19 @@ void vpImageConvert::YUV420ToRGB(unsigned char *yuv, unsigned char *rgb, unsigne const unsigned int halfHeight = height / 2, halfWidth = width / 2; for (unsigned int i = 0; i < halfHeight; ++i) { for (unsigned int j = 0; j < halfWidth; ++j) { - U = static_cast((*iU++ - 128) * 0.354); + U = static_cast(((*iU) - 128) * 0.354); + ++iU; U5 = 5 * U; - V = static_cast((*iV++ - 128) * 0.707); + V = static_cast(((*iV) - 128) * 0.707); + ++iV; V2 = 2 * V; UV = -U - V; - Y0 = *yuv++; + Y0 = *yuv; + ++yuv; Y1 = *yuv; yuv = yuv + (width - 1); - Y2 = *yuv++; + Y2 = *yuv; + ++yuv; Y3 = *yuv; yuv = (yuv - width) + 1; @@ -711,7 +756,8 @@ void vpImageConvert::YUV420ToRGB(unsigned char *yuv, unsigned char *rgb, unsigne void vpImageConvert::YUV420ToGrey(unsigned char *yuv, unsigned char *grey, unsigned int size) { for (unsigned int i = 0; i < size; ++i) { - *grey++ = *yuv++; + *grey++ = *yuv; + ++yuv; } } @@ -727,10 +773,13 @@ void vpImageConvert::YUV420ToGrey(unsigned char *yuv, unsigned char *grey, unsig void vpImageConvert::YUV444ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int size) { for (unsigned int i = 0; i < size; ++i) { - int U = static_cast((*yuv++ - 128) * 0.354); + int U = static_cast((*yuv - 128) * 0.354); + ++yuv; int U5 = 5 * U; - int Y = *yuv++; - int V = static_cast((*yuv++ - 128) * 0.707); + int Y = *yuv; + ++yuv; + int V = static_cast((*yuv - 128) * 0.707); + ++yuv; int V2 = 2 * V; int UV = -U - V; @@ -764,10 +813,13 @@ void vpImageConvert::YUV444ToRGBa(unsigned char *yuv, unsigned char *rgba, unsig void vpImageConvert::YUV444ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int size) { for (unsigned int i = 0; i < size; ++i) { - int U = static_cast((*yuv++ - 128) * 0.354); + int U = static_cast((*yuv - 128) * 0.354); + ++yuv; int U5 = 5 * U; - int Y = *yuv++; - int V = static_cast((*yuv++ - 128) * 0.707); + int Y = *yuv; + ++yuv; + int V = static_cast((*yuv - 128) * 0.707); + ++yuv; int V2 = 2 * V; int UV = -U - V; @@ -818,7 +870,6 @@ void vpImageConvert::YUV444ToGrey(unsigned char *yuv, unsigned char *grey, unsig */ void vpImageConvert::YV12ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int width, unsigned int height) { - // std::cout << "call optimized ConvertYV12ToRGBa()" << std::endl; int U, V, R, G, B, V2, U5, UV; int Y0, Y1, Y2, Y3; unsigned int size = width * height; @@ -827,15 +878,19 @@ void vpImageConvert::YV12ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne const unsigned int halfHeight = height / 2, halfWidth = width / 2; for (unsigned int i = 0; i < halfHeight; ++i) { for (unsigned int j = 0; j < halfWidth; ++j) { - U = static_cast((*iU++ - 128) * 0.354); + U = static_cast(((*iU) - 128) * 0.354); + ++iU; U5 = 5 * U; - V = static_cast((*iV++ - 128) * 0.707); + V = static_cast(((*iV) - 128) * 0.707); + ++iV; V2 = 2 * V; UV = -U - V; - Y0 = *yuv++; + Y0 = *yuv; + ++yuv; Y1 = *yuv; yuv = yuv + (width - 1); - Y2 = *yuv++; + Y2 = *yuv; + ++yuv; Y3 = *yuv; yuv = (yuv - width) + 1; @@ -919,7 +974,6 @@ void vpImageConvert::YV12ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne */ void vpImageConvert::YV12ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int height, unsigned int width) { - // std::cout << "call optimized ConvertYV12ToRGB()" << std::endl; int U, V, R, G, B, V2, U5, UV; int Y0, Y1, Y2, Y3; unsigned int size = width * height; @@ -928,15 +982,19 @@ void vpImageConvert::YV12ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned const unsigned int halfHeight = height / 2, halfWidth = width / 2; for (unsigned int i = 0; i < halfHeight; ++i) { for (unsigned int j = 0; j < halfWidth; ++j) { - U = static_cast((*iU++ - 128) * 0.354); + U = static_cast(((*iU) - 128) * 0.354); + ++iU; U5 = 5 * U; - V = static_cast((*iV++ - 128) * 0.707); + V = static_cast(((*iV) - 128) * 0.707); + ++iV; V2 = 2 * V; UV = -U - V; - Y0 = *yuv++; + Y0 = *yuv; + ++yuv; Y1 = *yuv; yuv = yuv + (width - 1); - Y2 = *yuv++; + Y2 = *yuv; + ++yuv; Y3 = *yuv; yuv = (yuv - width) + 1; @@ -1018,7 +1076,6 @@ void vpImageConvert::YV12ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned */ void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int width, unsigned int height) { - // std::cout << "call optimized ConvertYVU9ToRGBa()" << std::endl; int U, V, R, G, B, V2, U5, UV; int Y0, Y1, Y2, Y3, Y4, Y5, Y6, Y7, Y8, Y9, Y10, Y11, Y12, Y13, Y14, Y15; unsigned int size = width * height; @@ -1027,29 +1084,43 @@ void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne const unsigned int quarterHeight = height / 4, quarterWidth = width / 4; for (unsigned int i = 0; i < quarterHeight; ++i) { for (unsigned int j = 0; j < quarterWidth; ++j) { - U = static_cast((*iU++ - 128) * 0.354); + U = static_cast(((*iU) - 128) * 0.354); + ++iU; U5 = 5 * U; - V = static_cast((*iV++ - 128) * 0.707); + V = static_cast(((*iV) - 128) * 0.707); + ++iV; V2 = 2 * V; UV = -U - V; - Y0 = *yuv++; - Y1 = *yuv++; - Y2 = *yuv++; + Y0 = *yuv; + ++yuv; + Y1 = *yuv; + ++yuv; + Y2 = *yuv; + ++yuv; Y3 = *yuv; yuv = yuv + (width - 3); - Y4 = *yuv++; - Y5 = *yuv++; - Y6 = *yuv++; + Y4 = *yuv; + ++yuv; + Y5 = *yuv; + ++yuv; + Y6 = *yuv; + ++yuv; Y7 = *yuv; yuv = yuv + (width - 3); - Y8 = *yuv++; - Y9 = *yuv++; - Y10 = *yuv++; + Y8 = *yuv; + ++yuv; + Y9 = *yuv; + ++yuv; + Y10 = *yuv; + ++yuv; Y11 = *yuv; yuv = yuv + (width - 3); - Y12 = *yuv++; - Y13 = *yuv++; - Y14 = *yuv++; + Y12 = *yuv; + ++yuv; + Y13 = *yuv; + ++yuv; + Y14 = *yuv; + ++yuv; Y15 = *yuv; yuv = (yuv - (3 * width)) + 1; @@ -1311,7 +1382,6 @@ void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne */ void vpImageConvert::YVU9ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int height, unsigned int width) { - // std::cout << "call optimized ConvertYVU9ToRGB()" << std::endl; int U, V, R, G, B, V2, U5, UV; int Y0, Y1, Y2, Y3, Y4, Y5, Y6, Y7, Y8, Y9, Y10, Y11, Y12, Y13, Y14, Y15; unsigned int size = width * height; @@ -1320,29 +1390,43 @@ void vpImageConvert::YVU9ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned const unsigned int quarterHeight = height / 4, quarterWidth = width / 4; for (unsigned int i = 0; i < quarterHeight; ++i) { for (unsigned int j = 0; j < quarterWidth; ++j) { - U = static_cast((*iU++ - 128) * 0.354); + U = static_cast((*iU - 128) * 0.354); + ++iU; U5 = 5 * U; - V = static_cast((*iV++ - 128) * 0.707); + V = static_cast((*iV - 128) * 0.707); + ++iV; V2 = 2 * V; UV = -U - V; - Y0 = *yuv++; - Y1 = *yuv++; - Y2 = *yuv++; + Y0 = *yuv; + ++yuv; + Y1 = *yuv; + ++yuv; + Y2 = *yuv; + ++yuv; Y3 = *yuv; yuv = yuv + (width - 3); - Y4 = *yuv++; - Y5 = *yuv++; - Y6 = *yuv++; + Y4 = *yuv; + ++yuv; + Y5 = *yuv; + ++yuv; + Y6 = *yuv; + ++yuv; Y7 = *yuv; yuv = yuv + (width - 3); - Y8 = *yuv++; - Y9 = *yuv++; - Y10 = *yuv++; + Y8 = *yuv; + ++yuv; + Y9 = *yuv; + ++yuv; + Y10 = *yuv; + ++yuv; Y11 = *yuv; yuv = yuv + (width - 3); - Y12 = *yuv++; - Y13 = *yuv++; - Y14 = *yuv++; + Y12 = *yuv; + ++yuv; + Y13 = *yuv; + ++yuv; + Y14 = *yuv; + ++yuv; Y15 = *yuv; yuv = (yuv - (3 * width)) + 1; @@ -1516,7 +1600,7 @@ void vpImageConvert::YVU9ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned *rgb++ = static_cast(R); *rgb++ = static_cast(G); *rgb = static_cast(B); - rgb = rgb + 3 * width - 11; + rgb = (rgb + (3 * width)) - 11; R = Y12 + V2; vpSAT(R); diff --git a/modules/core/src/tools/file/vpIoTools.cpp b/modules/core/src/tools/file/vpIoTools.cpp index f4b25f22af..e00bd664d6 100644 --- a/modules/core/src/tools/file/vpIoTools.cpp +++ b/modules/core/src/tools/file/vpIoTools.cpp @@ -30,8 +30,7 @@ * * Description: * Directory management. - * -*****************************************************************************/ + */ /*! \file vpIoTools.cpp @@ -119,7 +118,7 @@ using namespace buminiz; #endif // To avoid warnings such as: warning: unused variable ‘littleEndian’ [-Wunused-variable] -#define _unused(x) ((void)(x)) // see: https://stackoverflow.com/a/777359 +#define UNUSED(x) ((void)(x)) // see: https://stackoverflow.com/a/777359 // Copyright (C) 2011 Carl Rogers // Released under MIT License @@ -130,41 +129,38 @@ using namespace buminiz; char visp::cnpy::BigEndianTest() { int x = 1; - return (((char *)&x)[0]) ? '<' : '>'; + return (((reinterpret_cast(&x))[0]) ? '<' : '>'); } char visp::cnpy::map_type(const std::type_info &t) { - if (t == typeid(float)) return 'f'; - if (t == typeid(double)) return 'f'; - if (t == typeid(long double)) return 'f'; + if (t == typeid(float)) { return 'f'; } + if (t == typeid(double)) { return 'f'; } + if (t == typeid(long double)) { return 'f'; } - if (t == typeid(int)) return 'i'; - if (t == typeid(char)) return 'i'; - if (t == typeid(short)) return 'i'; - if (t == typeid(long)) return 'i'; - if (t == typeid(long long)) return 'i'; + if (t == typeid(int)) { return 'i'; } + if (t == typeid(char)) { return 'i'; } + if (t == typeid(short)) { return 'i'; } + if (t == typeid(long)) { return 'i'; } + if (t == typeid(long long)) { return 'i'; } - if (t == typeid(unsigned char)) return 'u'; - if (t == typeid(unsigned short)) return 'u'; - if (t == typeid(unsigned long)) return 'u'; - if (t == typeid(unsigned long long)) return 'u'; - if (t == typeid(unsigned int)) return 'u'; + if (t == typeid(unsigned char)) { return 'u'; } + if (t == typeid(unsigned short)) { return 'u'; } + if (t == typeid(unsigned long)) { return 'u'; } + if (t == typeid(unsigned long long)) { return 'u'; } + if (t == typeid(unsigned int)) { return 'u'; } - if (t == typeid(bool)) return 'b'; + if (t == typeid(bool)) { return 'b'; } - if (t == typeid(std::complex)) return 'c'; - if (t == typeid(std::complex)) return 'c'; - if (t == typeid(std::complex)) return 'c'; + if (t == typeid(std::complex)) { return 'c'; } + if (t == typeid(std::complex)) { return 'c'; } + if (t == typeid(std::complex)) { return 'c'; } - else return '?'; + else { return '?'; } } void visp::cnpy::parse_npy_header(unsigned char *buffer, size_t &word_size, std::vector &shape, bool &fortran_order) { - //std::string magic_string(buffer,6); - // uint8_t major_version = *reinterpret_cast(buffer+6); - // uint8_t minor_version = *reinterpret_cast(buffer+7); uint16_t header_len = *reinterpret_cast(buffer+8); std::string header(reinterpret_cast(buffer+9), header_len); @@ -190,11 +186,8 @@ void visp::cnpy::parse_npy_header(unsigned char *buffer, size_t &word_size, std: //byte order code | stands for not applicable. //not sure when this applies except for byte array loc1 = header.find("descr")+9; - bool littleEndian = (header[loc1] == '<' || header[loc1] == '|' ? true : false); - _unused(littleEndian); assert(littleEndian); - - //char type = header[loc1+1]; - //assert(type == map_type(T)); + bool littleEndian = (((header[loc1] == '<') || (header[loc1] == '|')) ? true : false); + UNUSED(littleEndian); assert(littleEndian); std::string str_ws = header.substr(loc1+2); loc2 = str_ws.find("'"); @@ -205,8 +198,9 @@ void visp::cnpy::parse_npy_header(FILE *fp, size_t &word_size, std::vector footer(22); fseek(fp, -22, SEEK_END); size_t res = fread(&footer[0], sizeof(char), 22, fp); - if (res != 22) + if (res != 22) { throw std::runtime_error("parse_zip_footer: failed fread"); + } uint16_t disk_no, disk_start, nrecs_on_disk, comment_len; disk_no = *(uint16_t *)&footer[4]; @@ -270,10 +268,10 @@ void visp::cnpy::parse_zip_footer(FILE *fp, uint16_t &nrecs, size_t &global_head global_header_offset = *(uint32_t *)&footer[16]; comment_len = *(uint16_t *)&footer[20]; - _unused(disk_no); assert(disk_no == 0); - _unused(disk_start); assert(disk_start == 0); - _unused(nrecs_on_disk); assert(nrecs_on_disk == nrecs); - _unused(comment_len); assert(comment_len == 0); + UNUSED(disk_no); assert(disk_no == 0); + UNUSED(disk_start); assert(disk_start == 0); + UNUSED(nrecs_on_disk); assert(nrecs_on_disk == nrecs); + UNUSED(comment_len); assert(comment_len == 0); } visp::cnpy::NpyArray load_the_npy_file(FILE *fp) @@ -285,8 +283,9 @@ visp::cnpy::NpyArray load_the_npy_file(FILE *fp) visp::cnpy::NpyArray arr(shape, word_size, fortran_order); size_t nread = fread(arr.data(), 1, arr.num_bytes(), fp); - if (nread != arr.num_bytes()) + if (nread != arr.num_bytes()) { throw std::runtime_error("load_the_npy_file: failed fread"); + } return arr; } @@ -295,8 +294,9 @@ visp::cnpy::NpyArray load_the_npz_array(FILE *fp, uint32_t compr_bytes, uint32_t std::vector buffer_compr(compr_bytes); std::vector buffer_uncompr(uncompr_bytes); size_t nread = fread(&buffer_compr[0], 1, compr_bytes, fp); - if (nread != compr_bytes) + if (nread != compr_bytes) { throw std::runtime_error("load_the_npy_file: failed fread"); + } z_stream d_stream; @@ -306,7 +306,7 @@ visp::cnpy::NpyArray load_the_npz_array(FILE *fp, uint32_t compr_bytes, uint32_t d_stream.avail_in = 0; d_stream.next_in = Z_NULL; int err = inflateInit2(&d_stream, -MAX_WBITS); - _unused(err); assert(err == 0); + UNUSED(err); assert(err == 0); d_stream.avail_in = compr_bytes; d_stream.next_in = &buffer_compr[0]; @@ -314,9 +314,9 @@ visp::cnpy::NpyArray load_the_npz_array(FILE *fp, uint32_t compr_bytes, uint32_t d_stream.next_out = &buffer_uncompr[0]; err = inflate(&d_stream, Z_FINISH); - _unused(err); assert(err == 0); + UNUSED(err); assert(err == 0); err = inflateEnd(&d_stream); - _unused(err); assert(err == 0); + UNUSED(err); assert(err == 0); std::vector shape; size_t word_size; @@ -348,22 +348,26 @@ visp::cnpy::npz_t visp::cnpy::npz_load(std::string fname) } visp::cnpy::npz_t arrays; - - while (1) { + bool quit = false; + while (!quit) { std::vector local_header(30); size_t headerres = fread(&local_header[0], sizeof(char), 30, fp); - if (headerres != 30) + if (headerres != 30) { throw std::runtime_error("npz_load: failed fread"); + } //if we've reached the global header, stop reading - if (local_header[2] != 0x03 || local_header[3] != 0x04) break; + if ((local_header[2] != 0x03) || (local_header[3] != 0x04)) { + quit = true; + }; //read in the variable name uint16_t name_len = *(uint16_t *)&local_header[26]; std::string varname(name_len, ' '); size_t vname_res = fread(&varname[0], sizeof(char), name_len, fp); - if (vname_res != name_len) + if (vname_res != name_len) { throw std::runtime_error("npz_load: failed fread"); + } //erase the lagging .npy varname.erase(varname.end()-4, varname.end()); @@ -373,8 +377,9 @@ visp::cnpy::npz_t visp::cnpy::npz_load(std::string fname) if (extra_field_len > 0) { std::vector buff(extra_field_len); size_t efield_res = fread(&buff[0], sizeof(char), extra_field_len, fp); - if (efield_res != extra_field_len) + if (efield_res != extra_field_len) { throw std::runtime_error("npz_load: failed fread"); + } } uint16_t compr_method = *reinterpret_cast(&local_header[0]+8); @@ -402,23 +407,30 @@ visp::cnpy::NpyArray visp::cnpy::npz_load(std::string fname, std::string varname { FILE *fp = fopen(fname.c_str(), "rb"); - if (!fp) throw std::runtime_error("npz_load: Unable to open file "+fname); + if (!fp) { + throw std::runtime_error("npz_load: Unable to open file "+fname); + } - while (1) { + bool quit = false; + while (!quit) { std::vector local_header(30); size_t header_res = fread(&local_header[0], sizeof(char), 30, fp); - if (header_res != 30) + if (header_res != 30) { throw std::runtime_error("npz_load: failed fread"); + } - //if we've reached the global header, stop reading - if (local_header[2] != 0x03 || local_header[3] != 0x04) break; + //if we've reached the global header, stop reading + if (local_header[2] != 0x03 || local_header[3] != 0x04) { + quit = true; + }; //read in the variable name uint16_t name_len = *(uint16_t *)&local_header[26]; std::string vname(name_len, ' '); size_t vname_res = fread(&vname[0], sizeof(char), name_len, fp); - if (vname_res != name_len) + if (vname_res != name_len) { throw std::runtime_error("npz_load: failed fread"); + } vname.erase(vname.end()-4, vname.end()); //erase the lagging .npy //read in the extra field @@ -460,7 +472,9 @@ visp::cnpy::NpyArray visp::cnpy::npy_load(std::string fname) FILE *fp = fopen(fname.c_str(), "rb"); - if (!fp) throw std::runtime_error("npy_load: Unable to open file "+fname); + if (!fp) { + throw std::runtime_error("npy_load: Unable to open file "+fname); + } NpyArray arr = load_the_npy_file(fp); @@ -665,12 +679,12 @@ void vpIoTools::getUserName(std::string &username) // With MinGW, UNIX and _WIN32 are defined #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX // Get the user name. - char *_username = ::getenv("LOGNAME"); - if (!_username) { + char *logname = ::getenv("LOGNAME"); + if (!logname) { username = "unknown"; } else { - username = _username; + username = logname; } #elif defined(_WIN32) #if (!defined(WINRT)) @@ -829,18 +843,18 @@ bool vpIoTools::checkDirectory(const std::string &dirname) return false; } - std::string _dirname = path(dirname); + std::string path_dirname = path(dirname); - if (VP_STAT(_dirname.c_str(), &stbuf) != 0) { + if (VP_STAT(path_dirname.c_str(), &stbuf) != 0) { // Test adding the separator if not already present - if (_dirname.at(_dirname.size() - 1) != separator) { - if (VP_STAT((_dirname + separator).c_str(), &stbuf) != 0) { + if (path_dirname.at(path_dirname.size() - 1) != separator) { + if (VP_STAT((path_dirname + separator).c_str(), &stbuf) != 0) { return false; } } // Test removing the separator if already present - if (_dirname.at(_dirname.size() - 1) == separator) { - if (VP_STAT((_dirname.substr(0, _dirname.size() - 1)).c_str(), &stbuf) != 0) { + if (path_dirname.at(path_dirname.size() - 1) == separator) { + if (VP_STAT((path_dirname.substr(0, path_dirname.size() - 1)).c_str(), &stbuf) != 0) { return false; } } @@ -909,36 +923,36 @@ int vpIoTools::mkdir_p(const std::string &path, int mode) } // Iterate over the string - std::string _path = path; - std::string _sub_path; - for (size_t pos = 0; (pos = _path.find(vpIoTools::separator)) != std::string::npos;) { - _sub_path += _path.substr(0, pos + 1); + std::string cpy_path = path; + std::string sub_path; + for (size_t pos = 0; (pos = cpy_path.find(vpIoTools::separator)) != std::string::npos;) { + sub_path += cpy_path.substr(0, pos + 1); // Continue if sub_path = separator if (pos == 0) { - _path.erase(0, pos + 1); + cpy_path.erase(0, pos + 1); continue; } #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) - if (mkdir(_sub_path.c_str(), static_cast(mode)) != 0) + if (mkdir(sub_path.c_str(), static_cast(mode)) != 0) #elif defined(_WIN32) (void)mode; // var not used - if (!checkDirectory(_sub_path) && _mkdir(_sub_path.c_str()) != 0) + if (!checkDirectory(sub_path) && _mkdir(sub_path.c_str()) != 0) #endif { if (errno != EEXIST) { return -1; } } - _path.erase(0, pos + 1); + cpy_path.erase(0, pos + 1); } - if (!_path.empty()) { - _sub_path += _path; + if (!cpy_path.empty()) { + sub_path += cpy_path; #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) - if (mkdir(_sub_path.c_str(), static_cast(mode)) != 0) + if (mkdir(sub_path.c_str(), static_cast(mode)) != 0) #elif defined(_WIN32) - if (_mkdir(_sub_path.c_str()) != 0) + if (_mkdir(sub_path.c_str()) != 0) #endif { if (errno != EEXIST) { diff --git a/modules/vision/src/homography-estimation/vpHomography.cpp b/modules/vision/src/homography-estimation/vpHomography.cpp index 493db75ef9..c2f421fa6c 100644 --- a/modules/vision/src/homography-estimation/vpHomography.cpp +++ b/modules/vision/src/homography-estimation/vpHomography.cpp @@ -257,7 +257,7 @@ vpHomography &vpHomography::operator/=(double v) vpHomography &vpHomography::operator=(const vpHomography &H) { const unsigned int nbCols = 3, nbRows = 3; - for (unsigned int i = 0; i < nbRows; ++i){ + for (unsigned int i = 0; i < nbRows; ++i) { for (unsigned int j = 0; j < nbCols; ++j) { (*this)[i][j] = H[i][j]; } @@ -370,9 +370,9 @@ void vpHomography::build(vpHomography &aHb, const vpHomogeneousMatrix &aMb, cons double vpHomography::det() const { - return ((*this)[0][0] * (((*this)[1][1] * (*this)[2][2]) - ((*this)[1][2] * (*this)[2][1])) - - (*this)[0][1] * (((*this)[1][0] * (*this)[2][2]) - ((*this)[1][2] * (*this)[2][0])) + - (*this)[0][2] * (((*this)[1][0] * (*this)[2][1]) - ((*this)[1][1] * (*this)[2][0]))); + return ((((*this)[0][0] * (((*this)[1][1] * (*this)[2][2]) - ((*this)[1][2] * (*this)[2][1]))) - + ((*this)[0][1] * (((*this)[1][0] * (*this)[2][2]) - ((*this)[1][2] * (*this)[2][0])))) + + ((*this)[0][2] * (((*this)[1][0] * (*this)[2][1]) - ((*this)[1][1] * (*this)[2][0])))); } void vpHomography::eye() diff --git a/modules/vision/src/homography-estimation/vpHomographyRansac.cpp b/modules/vision/src/homography-estimation/vpHomographyRansac.cpp index 816b71785d..bd0e5fae50 100644 --- a/modules/vision/src/homography-estimation/vpHomographyRansac.cpp +++ b/modules/vision/src/homography-estimation/vpHomographyRansac.cpp @@ -95,42 +95,69 @@ bool vpHomography::degenerateConfiguration(const vpColVector &x, unsigned int *i i = 0, j = 1, k = 2; - double area012 = ((-pa[j][0] * pa[i][1]) + (pa[k][0] * pa[i][1]) + (pa[i][0] * pa[j][1]) - (pa[k][0] * pa[j][1]) + - (-pa[i][0] * pa[k][1]) + (pa[1][j] * pa[k][1])); + double area012 = ((((((-pa[j][0] * pa[i][1]) + (pa[k][0] * pa[i][1])) + (pa[i][0] * pa[j][1])) - (pa[k][0] * pa[j][1])) + + (-pa[i][0] * pa[k][1])) + (pa[1][j] * pa[k][1])); i = 0; j = 1, k = 3; - double area013 = ((-pa[j][0] * pa[i][1]) + (pa[k][0] * pa[i][1]) + (pa[i][0] * pa[j][1]) - (pa[k][0] * pa[j][1]) + - (-pa[i][0] * pa[k][1]) + (pa[1][j] * pa[k][1])); + double area013 = ((((((-pa[j][0] * pa[i][1]) + (pa[k][0] * pa[i][1])) + (pa[i][0] * pa[j][1])) - (pa[k][0] * pa[j][1])) + + (-pa[i][0] * pa[k][1])) + (pa[1][j] * pa[k][1])); i = 0; j = 2, k = 3; - double area023 = ((-pa[j][0] * pa[i][1]) + (pa[k][0] * pa[i][1]) + (pa[i][0] * pa[j][1]) - (pa[k][0] * pa[j][1]) + - (-pa[i][0] * pa[k][1]) + (pa[1][j] * pa[k][1])); + double area023 = ((((((-pa[j][0] * pa[i][1]) + (pa[k][0] * pa[i][1])) + (pa[i][0] * pa[j][1])) - (pa[k][0] * pa[j][1])) + + (-pa[i][0] * pa[k][1])) + (pa[1][j] * pa[k][1])); i = 1; j = 2, k = 3; - double area123 = ((-pa[j][0] * pa[i][1]) + (pa[k][0] * pa[i][1]) + (pa[i][0] * pa[j][1]) - (pa[k][0] * pa[j][1]) + - (-pa[i][0] * pa[k][1]) + (pa[1][j] * pa[k][1])); + double area123 = ((((((-pa[j][0] * pa[i][1]) + (pa[k][0] * pa[i][1])) + (pa[i][0] * pa[j][1])) - (pa[k][0] * pa[j][1])) + + (-pa[i][0] * pa[k][1])) + (pa[1][j] * pa[k][1])); double sum_area = area012 + area013 + area023 + area123; - return ((sum_area < threshold_area) || - (iscolinear(pa[0], pa[1], pa[2]) || iscolinear(pa[0], pa[1], pa[3]) || iscolinear(pa[0], pa[2], pa[3]) || - iscolinear(pa[1], pa[2], pa[3]) || iscolinear(pb[0], pb[1], pb[2]) || iscolinear(pb[0], pb[1], pb[3]) || - iscolinear(pb[0], pb[2], pb[3]) || iscolinear(pb[1], pb[2], pb[3]))); + if (sum_area < threshold_area) { + return true; + } + else if (iscolinear(pa[0], pa[1], pa[2])) { + return true; + } + else if (iscolinear(pa[0], pa[1], pa[3])) { + return true; + } + else if (iscolinear(pa[0], pa[2], pa[3])) { + return true; + } + else if (iscolinear(pa[1], pa[2], pa[3])) { + return true; + } + else if (iscolinear(pb[0], pb[1], pb[2])) { + return true; + } + else if (iscolinear(pb[0], pb[1], pb[3])) { + return true; + } + else if (iscolinear(pb[0], pb[2], pb[3])) { + return true; + } + else if (iscolinear(pb[1], pb[2], pb[3])) { + return true; + } + else { + return false; + } } + /* -\brief -Function to determine if a set of 4 pairs of matched points give rise -to a degeneracy in the calculation of a homography as needed by RANSAC. -This involves testing whether any 3 of the 4 points in each set is -colinear. - -point are coded this way -x1b,y1b, x2b, y2b, ... xnb, ynb -x1a,y1a, x2a, y2a, ... xna, yna -leading to 2*2*n + \brief + Function to determine if a set of 4 pairs of matched points give rise + to a degeneracy in the calculation of a homography as needed by RANSAC. + This involves testing whether any 3 of the 4 points in each set is + colinear. + + point are coded this way + x1b,y1b, x2b, y2b, ... xnb, ynb + x1a,y1a, x2a, y2a, ... xna, yna + leading to 2*2*n */ bool vpHomography::degenerateConfiguration(const vpColVector &x, unsigned int *ind) { @@ -156,10 +183,36 @@ bool vpHomography::degenerateConfiguration(const vpColVector &x, unsigned int *i pa[i][1] = x[n2 + ind2 + 1]; pa[i][2] = 1; } - return (iscolinear(pa[0], pa[1], pa[2]) || iscolinear(pa[0], pa[1], pa[3]) || iscolinear(pa[0], pa[2], pa[3]) || - iscolinear(pa[1], pa[2], pa[3]) || iscolinear(pb[0], pb[1], pb[2]) || iscolinear(pb[0], pb[1], pb[3]) || - iscolinear(pb[0], pb[2], pb[3]) || iscolinear(pb[1], pb[2], pb[3])); + + if (iscolinear(pa[0], pa[1], pa[2])) { + return true; + } + else if (iscolinear(pa[0], pa[1], pa[3])) { + return true; + } + else if (iscolinear(pa[0], pa[2], pa[3])) { + return true; + } + else if (iscolinear(pa[1], pa[2], pa[3])) { + return true; + } + else if (iscolinear(pb[0], pb[1], pb[2])) { + return true; + } + else if (iscolinear(pb[0], pb[1], pb[3])) { + return true; + } + else if (iscolinear(pb[0], pb[2], pb[3])) { + return true; + } + else if (iscolinear(pb[1], pb[2], pb[3])) { + return true; + } + else { + return false; + } } + bool vpHomography::degenerateConfiguration(const std::vector &xb, const std::vector &yb, const std::vector &xa, const std::vector &ya) { diff --git a/modules/vision/src/homography-estimation/vpHomographyVVS.cpp b/modules/vision/src/homography-estimation/vpHomographyVVS.cpp index d095bc0739..99e53f0fb4 100644 --- a/modules/vision/src/homography-estimation/vpHomographyVVS.cpp +++ b/modules/vision/src/homography-estimation/vpHomographyVVS.cpp @@ -69,7 +69,8 @@ static void updatePoseRotation(vpColVector &dx, vpHomogeneousMatrix &mati) rd[2][0] = (-sinu * u[1]) + (mcosi * u[2] * u[0]); rd[2][1] = (sinu * u[0]) + (mcosi * u[2] * u[1]); rd[2][2] = cosi + (mcosi * u[2] * u[2]); - } else { + } + else { for (unsigned int i = 0; i < 3; ++i) { for (unsigned int j = 0; j < 3; ++j) { rd[i][j] = 0.0; @@ -125,7 +126,8 @@ double vpHomography::computeRotation(unsigned int nbpoint, vpPoint *c1P, vpPoint W = 0; vpMatrix c2Rc1(3, 3); double r = 0; - while (vpMath::equal(r_1, r, m_threshold_rotation) == false) { + bool stop = false; + while ((vpMath::equal(r_1, r, m_threshold_rotation) == false) && (stop == false)) { r_1 = r; // compute current position @@ -191,23 +193,28 @@ double vpHomography::computeRotation(unsigned int nbpoint, vpPoint *c1P, vpPoint if (k == 0) { L = H2; e = e2; - } else { + } + else { L = vpMatrix::stack(L, H2); e = vpColVector::stack(e, e2); } - } else if (only_1) { + } + else if (only_1) { if (k == 0) { L = H1; e = e1; - } else { + } + else { L = vpMatrix::stack(L, H1); e = vpColVector::stack(e, e1); } - } else { + } + else { if (k == 0) { L = H2; e = e2; - } else { + } + else { L = vpMatrix::stack(L, H2); e = vpColVector::stack(e, e2); } @@ -221,7 +228,7 @@ double vpHomography::computeRotation(unsigned int nbpoint, vpPoint *c1P, vpPoint if (userobust) { for (unsigned int l = 0; l < n; ++l) { - res[l] = vpMath::sqr(e[2 * l]) + vpMath::sqr(e[(2 * l )+ 1]); + res[l] = vpMath::sqr(e[2 * l]) + vpMath::sqr(e[(2 * l)+ 1]); } robust.MEstimator(vpRobust::TUKEY, res, w); @@ -230,7 +237,8 @@ double vpHomography::computeRotation(unsigned int nbpoint, vpPoint *c1P, vpPoint W[2 * l][2 * l] = w[l]; W[(2 * l) + 1][(2 * l) + 1] = w[l]; } - } else { + } + else { for (unsigned int l = 0; l < (2 * n); ++l) { W[l][l] = 1; } @@ -250,7 +258,7 @@ double vpHomography::computeRotation(unsigned int nbpoint, vpPoint *c1P, vpPoint r = e.sumSquare(); if (((W * e).sumSquare() < 1e-10) || (iter > 25)) { - break; + stop = true; } ++iter; } @@ -307,7 +315,9 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP double r = 1e10; iter = 0; - while (!vpMath::equal(r_1, r, m_threshold_displacement)) { + vpMatrix sTR; + bool stop = false; + while ((!vpMath::equal(r_1, r, m_threshold_displacement)) && (stop == false)) { r_1 = r; // compute current position @@ -366,16 +376,16 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP H2 *= -1; vpMatrix c1CFc2(6, 6); - { - vpMatrix sTR = c1Tc2.skew() * static_cast(c1Rc2); - for (unsigned int k_ = 0; k_ < 3; ++k_) { - for (unsigned int l = 0; l < 3; ++l) { - c1CFc2[k_][l] = c1Rc2[k_][l]; - c1CFc2[k_ + 3][l + 3] = c1Rc2[k_][l]; - c1CFc2[k_][l + 3] = sTR[k_][l]; - } + + sTR = c1Tc2.skew() * c1Rc2; + for (unsigned int k_ = 0; k_ < 3; ++k_) { + for (unsigned int l = 0; l < 3; ++l) { + c1CFc2[k_][l] = c1Rc2[k_][l]; + c1CFc2[k_ + 3][l + 3] = c1Rc2[k_][l]; + c1CFc2[k_][l + 3] = sTR[k_][l]; } } + H2 = H2 * c1CFc2; // Set up the error vector @@ -408,23 +418,28 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP if (k == 0) { L = H2; e = e2; - } else { + } + else { L = vpMatrix::stack(L, H2); e = vpColVector::stack(e, e2); } - } else if (only_1) { + } + else if (only_1) { if (k == 0) { L = H1; e = e1; - } else { + } + else { L = vpMatrix::stack(L, H1); e = vpColVector::stack(e, e1); } - } else { + } + else { if (k == 0) { L = H2; e = e2; - } else { + } + else { L = vpMatrix::stack(L, H2); e = vpColVector::stack(e, e2); } @@ -437,7 +452,7 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP if (userobust) { for (unsigned int l = 0; l < n; ++l) { - res[l] = vpMath::sqr(e[2 * l]) + vpMath::sqr(e[(2 * l )+ 1]); + res[l] = vpMath::sqr(e[2 * l]) + vpMath::sqr(e[(2 * l)+ 1]); } robust.MEstimator(vpRobust::TUKEY, res, w); @@ -446,7 +461,8 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP W[2 * l][2 * l] = w[l]; W[(2 * l) + 1][(2 * l) + 1] = w[l]; } - } else { + } + else { for (unsigned int l = 0; l < (2 * n); ++l) { W[l][l] = 1; } @@ -463,7 +479,7 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP r = (W * e).sumSquare(); if ((r < 1e-15) || (iter > 1000) || (r > r_1)) { - break; + stop = true; } ++iter; } @@ -508,7 +524,9 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP double r = 1e10; iter = 0; - while (!vpMath::equal(r_1, r, m_threshold_displacement)) { + vpMatrix sTR; + bool stop = false; + while ((!vpMath::equal(r_1, r, m_threshold_displacement)) && (stop == false)) { r_1 = r; // compute current position @@ -566,16 +584,16 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP H2 *= -1; vpMatrix c1CFc2(6, 6); - { - vpMatrix sTR = c1Tc2.skew() * static_cast(c1Rc2); - for (unsigned int k_ = 0; k_ < 3; ++k_) { - for (unsigned int l = 0; l < 3; ++l) { - c1CFc2[k_][l] = c1Rc2[k_][l]; - c1CFc2[k_ + 3][l + 3] = c1Rc2[k_][l]; - c1CFc2[k_][l + 3] = sTR[k_][l]; - } + + sTR = c1Tc2.skew() * static_cast(c1Rc2); + for (unsigned int k_ = 0; k_ < 3; ++k_) { + for (unsigned int l = 0; l < 3; ++l) { + c1CFc2[k_][l] = c1Rc2[k_][l]; + c1CFc2[k_ + 3][l + 3] = c1Rc2[k_][l]; + c1CFc2[k_][l + 3] = sTR[k_][l]; } } + H2 = H2 * c1CFc2; // Set up the error vector @@ -608,23 +626,28 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP if (k == 0) { L = H2; e = e2; - } else { + } + else { L = vpMatrix::stack(L, H2); e = vpColVector::stack(e, e2); } - } else if (only_1) { + } + else if (only_1) { if (k == 0) { L = H1; e = e1; - } else { + } + else { L = vpMatrix::stack(L, H1); e = vpColVector::stack(e, e1); } - } else { + } + else { if (k == 0) { L = H2; e = e2; - } else { + } + else { L = vpMatrix::stack(L, H2); e = vpColVector::stack(e, e2); } @@ -646,7 +669,8 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP W[2 * k_][2 * k_] = w[k_]; W[(2 * k_) + 1][(2 * k_) + 1] = w[k_]; } - } else { + } + else { for (unsigned int k_ = 0; k_ < (2 * n); ++k_) { W[k_][k_] = 1; } @@ -663,7 +687,7 @@ double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpP r = (W * e).sumSquare(); if ((r < 1e-15) || (iter > 1000) || (r > r_1)) { - break; + stop = true; } ++iter; }