From c09f27d1a4b47e59db328e1263452ef795d98307 Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Tue, 18 Jun 2024 15:32:06 +0200 Subject: [PATCH 01/14] Fix quality rules to conform to misra c++ --- example/tracking/trackMeCircle.cpp | 9 +- modules/core/include/visp3/core/vpArray2D.h | 65 +- .../core/include/visp3/core/vpCPUFeatures.h | 6 +- .../include/visp3/core/vpCameraParameters.h | 30 +- .../include/visp3/core/vpCannyEdgeDetection.h | 6 +- modules/core/include/visp3/core/vpCircle.h | 33 +- modules/core/include/visp3/core/vpColVector.h | 6 +- modules/core/include/visp3/core/vpColor.h | 6 +- .../visp3/core/vpColorDepthConversion.h | 7 +- modules/core/include/visp3/core/vpDebug.h | 6 +- modules/core/include/visp3/core/vpDisplay.h | 6 +- .../include/visp3/core/vpDisplayException.h | 6 +- .../include/visp3/core/vpEigenConversion.h | 15 +- modules/core/include/visp3/core/vpEndian.h | 6 +- modules/core/include/visp3/core/vpException.h | 6 +- .../include/visp3/core/vpExponentialMap.h | 6 +- .../include/visp3/core/vpFeatureDisplay.h | 6 +- .../include/visp3/core/vpForceTwistMatrix.h | 72 +- .../include/visp3/core/vpForwardProjection.h | 6 +- .../core/include/visp3/core/vpFrameGrabber.h | 6 +- .../visp3/core/vpFrameGrabberException.h | 6 +- modules/core/include/visp3/core/vpGEMM.h | 6 +- .../include/visp3/core/vpGaussianFilter.h | 6 +- modules/core/include/visp3/core/vpHistogram.h | 10 +- .../core/include/visp3/core/vpHistogramPeak.h | 6 +- .../include/visp3/core/vpHistogramValey.h | 6 +- .../include/visp3/core/vpHomogeneousMatrix.h | 6 +- modules/core/include/visp3/core/vpImage.h | 58 +- .../core/include/visp3/core/vpImageCircle.h | 10 +- .../core/include/visp3/core/vpImageConvert.h | 7 +- .../include/visp3/core/vpImageException.h | 6 +- .../core/include/visp3/core/vpImageFilter.h | 34 +- .../include/visp3/core/vpImageMorphology.h | 6 +- .../core/include/visp3/core/vpImagePoint.h | 6 +- .../core/include/visp3/core/vpImageTools.h | 312 +++-- .../core/include/visp3/core/vpIoException.h | 6 +- modules/core/include/visp3/core/vpIoTools.h | 6 +- .../core/include/visp3/core/vpJsonParsing.h | 6 +- modules/core/include/visp3/core/vpLine.h | 6 +- modules/core/include/visp3/core/vpList.h | 6 +- modules/core/include/visp3/core/vpMath.h | 26 +- modules/core/include/visp3/core/vpMatrix.h | 72 +- .../include/visp3/core/vpMatrixException.h | 6 +- .../visp3/core/vpMeterPixelConversion.h | 18 +- .../core/include/visp3/core/vpMouseButton.h | 6 +- .../include/visp3/core/vpNullptrEmulated.h | 4 +- .../visp3/core/vpPixelMeterConversion.h | 64 +- modules/core/include/visp3/core/vpPlane.h | 41 +- modules/core/include/visp3/core/vpPoint.h | 6 +- modules/core/include/visp3/core/vpPolygon.h | 89 +- .../core/include/visp3/core/vpPoseVector.h | 6 +- .../include/visp3/core/vpQuaternionVector.h | 6 +- modules/core/include/visp3/core/vpRGBa.h | 6 +- modules/core/include/visp3/core/vpRGBf.h | 6 +- modules/core/include/visp3/core/vpRansac.h | 15 +- modules/core/include/visp3/core/vpRect.h | 6 +- .../core/include/visp3/core/vpRectOriented.h | 6 +- modules/core/include/visp3/core/vpRobust.h | 6 +- .../include/visp3/core/vpRotationMatrix.h | 6 +- .../include/visp3/core/vpRotationVector.h | 6 +- modules/core/include/visp3/core/vpRowVector.h | 6 +- .../core/include/visp3/core/vpRxyzVector.h | 6 +- .../core/include/visp3/core/vpRzyxVector.h | 6 +- .../core/include/visp3/core/vpRzyzVector.h | 6 +- modules/core/include/visp3/core/vpSphere.h | 27 +- .../core/include/visp3/core/vpSubColVector.h | 6 +- modules/core/include/visp3/core/vpSubMatrix.h | 6 +- .../core/include/visp3/core/vpSubRowVector.h | 6 +- .../core/include/visp3/core/vpThetaUVector.h | 6 +- modules/core/include/visp3/core/vpTime.h | 6 +- modules/core/include/visp3/core/vpTracker.h | 6 +- .../include/visp3/core/vpTrackingException.h | 6 +- .../include/visp3/core/vpTranslationVector.h | 8 +- modules/core/include/visp3/core/vpUniRand.h | 12 +- .../visp3/core/vpVelocityTwistMatrix.h | 6 +- .../include/visp3/core/vpXmlParserCamera.h | 8 +- .../visp3/core/vpXmlParserHomogeneousMatrix.h | 6 +- .../visp3/core/vpXmlParserRectOriented.h | 6 +- .../core/src/camera/vpCameraParameters.cpp | 68 +- .../src/camera/vpColorDepthConversion.cpp | 48 +- .../src/camera/vpMeterPixelConversion.cpp | 82 +- .../src/camera/vpPixelMeterConversion.cpp | 14 +- modules/core/src/camera/vpXmlParserCamera.cpp | 744 +++++----- modules/core/src/display/vpColor.cpp | 5 +- modules/core/src/display/vpDisplay.cpp | 5 +- modules/core/src/display/vpDisplay_impl.h | 33 +- modules/core/src/display/vpDisplay_rgba.cpp | 397 +++--- modules/core/src/display/vpDisplay_uchar.cpp | 399 +++--- modules/core/src/display/vpFeatureDisplay.cpp | 38 +- .../core/src/framegrabber/vpFrameGrabber.cpp | 2 +- .../src/image/private/vpImageConvert_impl.h | 14 +- .../core/src/image/vpCannyEdgeDetection.cpp | 32 +- modules/core/src/image/vpGaussianFilter.cpp | 7 +- modules/core/src/image/vpImageCircle.cpp | 78 +- modules/core/src/image/vpImageConvert.cpp | 63 +- modules/core/src/image/vpImageConvert_hsv.cpp | 37 +- modules/core/src/image/vpImageConvert_pcl.cpp | 47 +- modules/core/src/image/vpImageConvert_yuv.cpp | 248 ++-- modules/core/src/image/vpImageFilter.cpp | 19 +- modules/core/src/image/vpImagePoint.cpp | 5 +- modules/core/src/image/vpImageTools.cpp | 118 +- modules/core/src/image/vpRGBa.cpp | 79 +- modules/core/src/image/vpRGBf.cpp | 65 +- modules/core/src/math/matrix/vpColVector.cpp | 30 +- .../src/math/matrix/vpEigenConversion.cpp | 9 +- modules/core/src/math/matrix/vpMatrix.cpp | 13 +- .../src/math/matrix/vpMatrix_cholesky.cpp | 8 +- .../src/math/matrix/vpMatrix_covariance.cpp | 35 +- modules/core/src/math/matrix/vpMatrix_lu.cpp | 287 ++-- modules/core/src/math/matrix/vpMatrix_mul.cpp | 5 +- modules/core/src/math/matrix/vpMatrix_qr.cpp | 280 ++-- modules/core/src/math/matrix/vpMatrix_svd.cpp | 82 +- modules/core/src/math/matrix/vpRowVector.cpp | 73 +- .../core/src/math/matrix/vpSubColVector.cpp | 11 +- modules/core/src/math/matrix/vpSubMatrix.cpp | 11 +- .../core/src/math/matrix/vpSubRowVector.cpp | 2 +- modules/core/src/math/misc/vpMath.cpp | 103 +- .../src/math/random-generator/vpUniRand.cpp | 5 +- modules/core/src/math/robust/vpRobust.cpp | 21 +- .../math/transformation/vpExponentialMap.cpp | 162 +-- .../transformation/vpForceTwistMatrix.cpp | 97 +- .../transformation/vpHomogeneousMatrix.cpp | 250 ++-- .../src/math/transformation/vpPoseVector.cpp | 104 +- .../transformation/vpQuaternionVector.cpp | 104 +- .../math/transformation/vpRotationMatrix.cpp | 354 ++--- .../math/transformation/vpRotationVector.cpp | 5 +- .../src/math/transformation/vpRxyzVector.cpp | 34 +- .../src/math/transformation/vpRzyxVector.cpp | 40 +- .../src/math/transformation/vpRzyzVector.cpp | 32 +- .../math/transformation/vpThetaUVector.cpp | 76 +- .../transformation/vpTranslationVector.cpp | 83 +- .../transformation/vpVelocityTwistMatrix.cpp | 67 +- .../vpXmlParserHomogeneousMatrix.cpp | 197 ++- .../src/tools/cpu-features/vpCPUFeatures.cpp | 5 +- .../src/tools/cpu-features/x86/cpu_x86.cpp | 108 +- .../core/src/tools/cpu-features/x86/cpu_x86.h | 6 +- modules/core/src/tools/endian/vpEndian.cpp | 41 +- .../core/src/tools/exceptions/vpException.cpp | 6 +- modules/core/src/tools/file/vpIoTools.cpp | 105 +- modules/core/src/tools/geometry/vpPlane.cpp | 131 +- modules/core/src/tools/geometry/vpPolygon.cpp | 5 +- modules/core/src/tools/geometry/vpRect.cpp | 6 +- .../src/tools/geometry/vpRectOriented.cpp | 5 +- .../geometry/vpXmlParserRectOriented.cpp | 11 +- .../core/src/tools/histogram/vpHistogram.cpp | 9 +- .../src/tools/histogram/vpHistogramPeak.cpp | 5 +- .../src/tools/histogram/vpHistogramValey.cpp | 5 +- modules/core/src/tools/time/vpTime.cpp | 30 +- .../tracking/forward-projection/vpCircle.cpp | 190 ++- .../vpForwardProjection.cpp | 6 +- .../tracking/forward-projection/vpLine.cpp | 121 +- .../tracking/forward-projection/vpPoint.cpp | 289 ++-- .../tracking/forward-projection/vpSphere.cpp | 89 +- modules/core/src/tracking/vpTracker.cpp | 6 +- .../test/tools/geometry/testImageCircle.cpp | 290 ++-- .../visp3/detection/vpDetectorAprilTag.h | 29 +- .../include/visp3/detection/vpDetectorBase.h | 26 +- .../detection/src/tag/vpDetectorAprilTag.cpp | 140 +- modules/detection/src/vpDetectorBase.cpp | 5 +- modules/gui/include/visp3/gui/vpD3DRenderer.h | 6 +- modules/gui/include/visp3/gui/vpDisplayD3D.h | 6 +- modules/gui/include/visp3/gui/vpDisplayGDI.h | 18 +- modules/gui/include/visp3/gui/vpDisplayGTK.h | 6 +- .../gui/include/visp3/gui/vpDisplayOpenCV.h | 6 +- .../gui/include/visp3/gui/vpDisplayWin32.h | 6 +- modules/gui/include/visp3/gui/vpDisplayX.h | 6 +- modules/gui/include/visp3/gui/vpGDIRenderer.h | 16 +- modules/gui/include/visp3/gui/vpPlot.h | 6 +- modules/gui/include/visp3/gui/vpPlotCurve.h | 19 +- modules/gui/include/visp3/gui/vpPlotGraph.h | 12 +- .../include/visp3/gui/vpProjectionDisplay.h | 12 +- modules/gui/include/visp3/gui/vpWin32API.h | 15 +- .../gui/include/visp3/gui/vpWin32Renderer.h | 17 +- modules/gui/include/visp3/gui/vpWin32Window.h | 16 +- modules/gui/src/display/vpDisplayGTK.cpp | 22 +- modules/gui/src/display/vpDisplayOpenCV.cpp | 28 +- modules/gui/src/display/vpDisplayX.cpp | 50 +- .../gui/src/display/windows/vpD3DRenderer.cpp | 48 +- .../gui/src/display/windows/vpDisplayD3D.cpp | 11 +- .../gui/src/display/windows/vpDisplayGDI.cpp | 11 +- .../src/display/windows/vpDisplayWin32.cpp | 18 +- .../gui/src/display/windows/vpGDIRenderer.cpp | 12 +- .../gui/src/display/windows/vpWin32API.cpp | 11 +- .../gui/src/display/windows/vpWin32Window.cpp | 13 +- .../vpProjectionDisplay.cpp | 5 +- modules/gui/src/plot/vpPlot.cpp | 35 +- modules/gui/src/plot/vpPlotCurve.cpp | 5 +- modules/gui/src/plot/vpPlotGraph.cpp | 23 +- .../visp3/imgproc/vpCircleHoughTransform.h | 88 +- .../include/visp3/imgproc/vpContours.h | 45 +- .../imgproc/include/visp3/imgproc/vpImgproc.h | 6 +- modules/imgproc/src/vpCLAHE.cpp | 97 +- .../imgproc/src/vpCircleHoughTransform.cpp | 259 ++-- modules/imgproc/src/vpConnectedComponents.cpp | 2 +- modules/imgproc/src/vpContours.cpp | 7 +- modules/imgproc/src/vpFloodFill.cpp | 2 +- modules/imgproc/src/vpImgproc.cpp | 99 +- modules/imgproc/src/vpMorph.cpp | 5 +- modules/imgproc/src/vpRetinex.cpp | 47 +- modules/imgproc/src/vpThreshold.cpp | 31 +- modules/io/include/visp3/io/vpDiskGrabber.h | 7 +- modules/io/include/visp3/io/vpImageIo.h | 7 +- .../include/visp3/io/vpJsonArgumentParser.h | 8 +- modules/io/include/visp3/io/vpParseArgv.h | 4 +- modules/io/include/visp3/io/vpVideoReader.h | 6 +- modules/io/include/visp3/io/vpVideoWriter.h | 6 +- .../io/src/image/private/vpImageIoBackend.h | 2 +- .../io/src/image/private/vpImageIoLibjpeg.cpp | 2 +- .../io/src/image/private/vpImageIoLibpng.cpp | 14 +- .../io/src/image/private/vpImageIoOpenCV.cpp | 2 +- .../src/image/private/vpImageIoPortable.cpp | 22 +- .../io/src/image/private/vpImageIoSimd.cpp | 2 +- modules/io/src/image/private/vpImageIoStb.cpp | 2 +- .../io/src/image/private/vpImageIoTinyEXR.cpp | 2 +- modules/io/src/image/vpImageIo.cpp | 2 +- modules/io/src/tools/vpJsonArgumentParser.cpp | 35 +- modules/io/src/tools/vpParseArgv.cpp | 47 +- modules/io/src/video/vpDiskGrabber.cpp | 2 +- modules/io/src/video/vpVideoReader.cpp | 25 +- modules/io/src/video/vpVideoWriter.cpp | 3 +- .../framegrabber/1394/vp1394TwoGrabber.cpp | 81 +- .../tracker/blob/include/visp3/blob/vpDot.h | 186 ++- .../tracker/blob/include/visp3/blob/vpDot2.h | 153 ++- modules/tracker/blob/src/dots/vpDot.cpp | 506 ++++--- modules/tracker/blob/src/dots/vpDot2.cpp | 1202 +++++++++-------- .../mbt/include/visp3/mbt/vpMbHiddenFaces.h | 7 +- modules/tracker/mbt/src/edge/vpMbtMeLine.cpp | 11 +- modules/tracker/me/include/visp3/me/vpMe.h | 6 +- .../tracker/me/include/visp3/me/vpMeEllipse.h | 24 +- .../tracker/me/include/visp3/me/vpMeSite.h | 6 +- .../tracker/me/include/visp3/me/vpMeTracker.h | 4 +- modules/tracker/me/src/moving-edges/vpMe.cpp | 224 +-- .../me/src/moving-edges/vpMeEllipse.cpp | 303 ++--- .../tracker/me/src/moving-edges/vpMeLine.cpp | 1 + .../tracker/me/src/moving-edges/vpMeSite.cpp | 22 +- .../me/src/moving-edges/vpMeTracker.cpp | 13 +- .../tt/src/warp/vpTemplateTrackerWarp.cpp | 14 +- .../include/visp3/vision/vpBasicKeyPoint.h | 7 +- .../include/visp3/vision/vpHomography.h | 6 +- modules/vision/include/visp3/vision/vpPose.h | 31 +- .../include/visp3/vision/vpPoseException.h | 6 +- .../src/calibration/vpCalibrationTools.cpp | 11 +- .../homography-estimation/vpHomography.cpp | 2 +- .../homography-estimation/vpHomographyDLT.cpp | 16 +- .../vpHomographyExtract.cpp | 75 +- .../vpHomographyMalis.cpp | 27 +- .../vpHomographyRansac.cpp | 25 +- .../homography-estimation/vpHomographyVVS.cpp | 16 +- .../private/vpLevenbergMarquartd.cpp | 10 +- .../private/vpLevenbergMarquartd.h | 62 +- modules/vision/src/pose-estimation/vpPose.cpp | 137 +- .../src/pose-estimation/vpPoseDementhon.cpp | 304 ++--- .../src/pose-estimation/vpPoseLagrange.cpp | 559 +++----- .../vision/src/pose-estimation/vpPoseLowe.cpp | 182 +-- .../vision/src/pose-estimation/vpPoseRGBD.cpp | 79 +- .../src/pose-estimation/vpPoseRansac.cpp | 177 +-- .../vpPoseVirtualVisualServoing.cpp | 326 ++--- .../visp3/visual_features/vpFeatureSegment.h | 7 +- .../feature-builder/vpFeatureBuilderPoint.cpp | 9 +- .../vpFeatureBuilderPointPolar.cpp | 9 +- .../src/visual-feature/vpFeatureLuminance.cpp | 1 + .../vpFeatureMomentCentered.cpp | 3 +- modules/vs/src/vpServoData.cpp | 7 +- 263 files changed, 7430 insertions(+), 7034 deletions(-) diff --git a/example/tracking/trackMeCircle.cpp b/example/tracking/trackMeCircle.cpp index 2597ed0538..3beca9c4f2 100644 --- a/example/tracking/trackMeCircle.cpp +++ b/example/tracking/trackMeCircle.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Tracking of an ellipse. - * -*****************************************************************************/ + */ /*! \file trackMeCircle.cpp @@ -46,6 +44,7 @@ */ #include +#include #include #include diff --git a/modules/core/include/visp3/core/vpArray2D.h b/modules/core/include/visp3/core/vpArray2D.h index 3137c6a8a2..6e5b62b950 100644 --- a/modules/core/include/visp3/core/vpArray2D.h +++ b/modules/core/include/visp3/core/vpArray2D.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +30,8 @@ * Description: * This class implements an 2D array as a template class. */ -#ifndef _vpArray2D_h_ -#define _vpArray2D_h_ +#ifndef VP_ARRAY2D_H +#define VP_ARRAY2D_H #include #include @@ -125,26 +125,15 @@ BEGIN_VISP_NAMESPACE */ template class vpArray2D { -protected: - //! Number of rows in the array - unsigned int rowNum; - //! Number of columns in the array - unsigned int colNum; - //! Address of the first element of each rows - Type **rowPtrs; - //! Current array size (rowNum * colNum) - unsigned int dsize; - public: //! Address of the first element of the data array Type *data; -public: /*! * Basic constructor of a 2D array. * Number of columns and rows are set to zero. */ - vpArray2D() : rowNum(0), colNum(0), rowPtrs(nullptr), dsize(0), data(nullptr) { } + vpArray2D() : data(nullptr), rowNum(0), colNum(0), rowPtrs(nullptr), dsize(0) { } /*! Copy constructor of a 2D array. @@ -154,7 +143,7 @@ template class vpArray2D #if ((__cplusplus >= 201103L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201103L))) // Check if cxx11 or higher vpArray2D() #else - rowNum(0), colNum(0), rowPtrs(nullptr), dsize(0), data(nullptr) + data(nullptr), rowNum(0), colNum(0), rowPtrs(nullptr), dsize(0) #endif { resize(A.rowNum, A.colNum, false, false); @@ -172,7 +161,7 @@ template class vpArray2D #if ((__cplusplus >= 201103L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201103L))) // Check if cxx11 or higher vpArray2D() #else - rowNum(0), colNum(0), rowPtrs(nullptr), dsize(0), data(nullptr) + data(nullptr), rowNum(0), colNum(0), rowPtrs(nullptr), dsize(0) #endif { resize(r, c); @@ -190,7 +179,7 @@ template class vpArray2D #if ((__cplusplus >= 201103L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201103L))) // Check if cxx11 or higher vpArray2D() #else - rowNum(0), colNum(0), rowPtrs(nullptr), dsize(0), data(nullptr) + data(nullptr), rowNum(0), colNum(0), rowPtrs(nullptr), dsize(0) #endif { resize(r, c, false, false); @@ -213,7 +202,7 @@ template class vpArray2D #if ((__cplusplus >= 201103L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201103L))) // Check if cxx11 or higher vpArray2D() #else - rowNum(0), colNum(0), rowPtrs(nullptr), dsize(0), data(nullptr) + data(nullptr), rowNum(0), colNum(0), rowPtrs(nullptr), dsize(0) #endif { if ((r > 0) && (c > 0)) { @@ -272,7 +261,7 @@ template class vpArray2D } explicit vpArray2D(unsigned int nrows, unsigned int ncols, const std::initializer_list &list) - : rowNum(0), colNum(0), rowPtrs(nullptr), dsize(0), data(nullptr) + : data(nullptr), rowNum(0), colNum(0), rowPtrs(nullptr), dsize(0) { if ((nrows * ncols) != static_cast(list.size())) { std::ostringstream oss; @@ -607,8 +596,8 @@ template class vpArray2D return s; } std::ios_base::fmtflags original_flags = s.flags(); - - s.precision(10); + const unsigned int precision = 10; + s.precision(precision); unsigned int a_rows = A.getRows(); unsigned int a_cols = A.getCols(); for (unsigned int i = 0; i < a_rows; ++i) { @@ -677,18 +666,18 @@ template class vpArray2D bool headerIsDecoded = false; do { std::streampos pos = file.tellg(); - char line[256]; - file.getline(line, 256); + char line[FILENAME_MAX]; + file.getline(line, FILENAME_MAX); std::string prefix("# "); std::string line_(line); - if (line_.compare(0, 2, prefix.c_str()) == 0) { + if (line_.compare(0, prefix.size(), prefix.c_str()) == 0) { // Line is a comment // If we are not on the first line, we should add "\n" to the end of // the previous line if (pos) { h += "\n"; } - h += line_.substr(2); // Remove "# " + h += line_.substr(prefix.size()); // Remove "# " } else { // rewind before the line @@ -758,6 +747,7 @@ template class vpArray2D file.close(); return true; } + /*! Load an array from a YAML-formatted file. @@ -768,7 +758,6 @@ template class vpArray2D \return Returns true on success. \sa saveYAML() - */ static bool loadYAML(const std::string &filename, vpArray2D &A, char *header = nullptr) { @@ -790,17 +779,20 @@ template class vpArray2D while (getline(file, line)) { if (inheader) { - if ((rows == 0) && (line.compare(0, 5, "rows:") == 0)) { + const std::string str_rows("rows:"); + const std::string str_cols("cols:"); + const std::string str_data("data:"); + if ((rows == 0) && (line.compare(0, str_rows.size(), str_rows.c_str()) == 0)) { std::stringstream ss(line); ss >> subs; ss >> rows; } - else if ((cols == 0) && (line.compare(0, 5, "cols:") == 0)) { + else if ((cols == 0) && (line.compare(0, str_cols.size(), str_cols.c_str()) == 0)) { std::stringstream ss(line); ss >> subs; ss >> cols; } - else if (line.compare(0, 5, "data:") == 0) { + else if (line.compare(0, str_data.size(), str_data.c_str()) == 0) { inheader = false; } else { @@ -1082,6 +1074,16 @@ template class vpArray2D */ static void insert(const vpArray2D &A, const vpArray2D &B, vpArray2D &C, unsigned int r, unsigned int c); //@} + +protected: + //! Number of rows in the array + unsigned int rowNum; + //! Number of columns in the array + unsigned int colNum; + //! Address of the first element of each rows + Type **rowPtrs; + //! Current array size (rowNum * colNum) + unsigned int dsize; }; /*! @@ -1175,7 +1177,8 @@ template void vpArray2D::conv2(const vpArray2D &M, cons if ((mode == "full") || (mode == "same")) { const unsigned int pad_x = kernel.getCols() - 1; const unsigned int pad_y = kernel.getRows() - 1; - M_padded.resize(M.getRows() + (2 * pad_y), M.getCols() + (2 * pad_x), true, false); + const unsigned int pad = 2; + M_padded.resize(M.getRows() + (pad * pad_y), M.getCols() + (pad * pad_x), true, false); M_padded.insert(M, pad_y, pad_x); if (mode == "same") { diff --git a/modules/core/include/visp3/core/vpCPUFeatures.h b/modules/core/include/visp3/core/vpCPUFeatures.h index 7947b8fe0b..611f3eb75d 100644 --- a/modules/core/include/visp3/core/vpCPUFeatures.h +++ b/modules/core/include/visp3/core/vpCPUFeatures.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,8 +36,8 @@ \brief Check CPU features (hardware capabilities). */ -#ifndef _vpCPUFeatures_h_ -#define _vpCPUFeatures_h_ +#ifndef VP_CPU_FEATURES_H +#define VP_CPU_FEATURES_H #include diff --git a/modules/core/include/visp3/core/vpCameraParameters.h b/modules/core/include/visp3/core/vpCameraParameters.h index ef165a8b20..5c1206097d 100644 --- a/modules/core/include/visp3/core/vpCameraParameters.h +++ b/modules/core/include/visp3/core/vpCameraParameters.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,22 +31,21 @@ * Camera intrinsic parameters. */ -/*! - \file vpCameraParameters.h - \brief Declaration of the vpCameraParameters class. - Class vpCameraParameters define the camera intrinsic parameters + /*! + \file vpCameraParameters.h + \brief Declaration of the vpCameraParameters class. + Class vpCameraParameters define the camera intrinsic parameters -*/ + */ -#ifndef _vpCameraParameters_h_ -#define _vpCameraParameters_h_ +#ifndef VP_CAMERA_PARAMETERS_H +#define VP_CAMERA_PARAMETERS_H #include #include -#include #include -#include +#include #include #ifdef VISP_HAVE_NLOHMANN_JSON @@ -358,8 +357,7 @@ class VISP_EXPORT vpCameraParameters inline double getHorizontalFovAngle() const { if (!m_isFov) { - vpTRACE("Warning: The FOV is not computed, getHorizontalFovAngle() " - "won't be significant."); + std::cout << "Warning: The FOV is not computed, getHorizontalFovAngle() won't be significant." << std::endl; } return m_hFovAngle; } @@ -374,8 +372,7 @@ class VISP_EXPORT vpCameraParameters inline double getVerticalFovAngle() const { if (!m_isFov) { - vpTRACE("Warning: The FOV is not computed, getVerticalFovAngle() won't " - "be significant."); + std::cout << "Warning: The FOV is not computed, getVerticalFovAngle() won't be significant." << std::endl; } return m_vFovAngle; } @@ -395,8 +392,7 @@ class VISP_EXPORT vpCameraParameters inline std::vector getFovNormals() const { if (!m_isFov) { - vpTRACE("Warning: The FOV is not computed, getFovNormals() won't be " - "significant."); + std::cout << "Warning: The FOV is not computed, getFovNormals() won't be significant." << std::endl; } return m_fovNormals; } @@ -544,6 +540,8 @@ inline void from_json(const nlohmann::json &j, vpCameraParameters &cam) cam.initProjWithKannalaBrandtDistortion(px, py, u0, v0, coeffs); break; } + default: + break; } } #endif diff --git a/modules/core/include/visp3/core/vpCannyEdgeDetection.h b/modules/core/include/visp3/core/vpCannyEdgeDetection.h index 723de821e4..30d3989628 100644 --- a/modules/core/include/visp3/core/vpCannyEdgeDetection.h +++ b/modules/core/include/visp3/core/vpCannyEdgeDetection.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -28,8 +28,8 @@ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. */ -#ifndef _vpCannyEdgeDetection_h_ -#define _vpCannyEdgeDetection_h_ +#ifndef VP_CANNY_EDGE_DETECTION_H +#define VP_CANNY_EDGE_DETECTION_H // System includes #include diff --git a/modules/core/include/visp3/core/vpCircle.h b/modules/core/include/visp3/core/vpCircle.h index 71b4361107..d7c6428344 100644 --- a/modules/core/include/visp3/core/vpCircle.h +++ b/modules/core/include/visp3/core/vpCircle.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,12 +36,11 @@ \brief class that defines what is a circle */ -#ifndef _vpCircle_h_ -#define _vpCircle_h_ +#ifndef VP_CIRCLE_H +#define VP_CIRCLE_H #include #include -#include #include #include #include @@ -109,19 +108,19 @@ class VISP_EXPORT vpCircle : public vpForwardProjection double get_x() const { return p[0]; } double get_y() const { return p[1]; } - double get_n20() const { return p[2]; } - double get_n11() const { return p[3]; } - double get_n02() const { return p[4]; } + double get_n20() const { const unsigned int index_2 = 2; return p[index_2]; } + double get_n11() const { const unsigned int index_3 = 3; return p[index_3]; } + double get_n02() const { const unsigned int index_4 = 4; return p[index_4]; } - double getA() const { return cP[0]; } - double getB() const { return cP[1]; } - double getC() const { return cP[2]; } + double getA() const { const unsigned int index_0 = 0; return cP[index_0]; } + double getB() const { const unsigned int index_1 = 1; return cP[index_1]; } + double getC() const { const unsigned int index_2 = 2; return cP[index_2]; } - double getX() const { return cP[3]; } - double getY() const { return cP[4]; } - double getZ() const { return cP[5]; } + double getX() const { const unsigned int index_3 = 3; return cP[index_3]; } + double getY() const { const unsigned int index_4 = 4; return cP[index_4]; } + double getZ() const { const unsigned int index_5 = 5; return cP[index_5]; } - double getR() const { return cP[6]; } + double getR() const { const unsigned int index_6 = 6; return cP[index_6]; } void projection() vp_override; void projection(const vpColVector &cP, vpColVector &p) const vp_override; @@ -147,21 +146,21 @@ class VISP_EXPORT vpCircle : public vpForwardProjection * returns second order centered moments of the ellipse normalized * by its area that corresponds to \f$n_20 = mu_20/a\f$. */ - vp_deprecated double get_mu20() const { return p[2]; } + vp_deprecated double get_mu20() const { const unsigned int index_2 = 2; return p[index_2]; } /*! * \deprecated You should rather use get_n11(). * This function is incorrectly named and is confusing since it * returns second order centered moments of the ellipse normalized * by its area that corresponds to \f$n_11 = mu@name Deprecated functions_11/a\f$. */ - vp_deprecated double get_mu11() const { return p[3]; } + vp_deprecated double get_mu11() const { const unsigned int index_3 = 3; return p[index_3]; } /*! * \deprecated You should rather use get_n02(). * This function is incorrectly named and is confusing since it * returns second order centered moments of the ellipse normalized * by its area that corresponds to \f$n_02 = mu_02/a\f$. */ - vp_deprecated double get_mu02() const { return p[4]; } + vp_deprecated double get_mu02() const { const unsigned int index_4 = 4; return p[index_4]; } //@} #endif protected: diff --git a/modules/core/include/visp3/core/vpColVector.h b/modules/core/include/visp3/core/vpColVector.h index b78e6b3d32..8875575573 100644 --- a/modules/core/include/visp3/core/vpColVector.h +++ b/modules/core/include/visp3/core/vpColVector.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -37,8 +37,8 @@ * as a set of operations on these vector */ -#ifndef _vpColVector_h_ -#define _vpColVector_h_ +#ifndef VP_COLVECTOR_H +#define VP_COLVECTOR_H #include diff --git a/modules/core/include/visp3/core/vpColor.h b/modules/core/include/visp3/core/vpColor.h index 4e42fec9a9..148f8b12a6 100644 --- a/modules/core/include/visp3/core/vpColor.h +++ b/modules/core/include/visp3/core/vpColor.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,8 +31,8 @@ * Color definition. */ -#ifndef vpColor_hh -#define vpColor_hh +#ifndef VP_COLOR_H +#define VP_COLOR_H #include #include diff --git a/modules/core/include/visp3/core/vpColorDepthConversion.h b/modules/core/include/visp3/core/vpColorDepthConversion.h index f5dfb51185..5ec41ef883 100644 --- a/modules/core/include/visp3/core/vpColorDepthConversion.h +++ b/modules/core/include/visp3/core/vpColorDepthConversion.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,7 +31,8 @@ * Color to Depth conversion. */ -#pragma once +#ifndef VP_COLOR_DEPTH_CONVERSION_H +#define VP_COLOR_DEPTH_CONVERSION_H #include @@ -56,3 +57,5 @@ class VISP_EXPORT vpColorDepthConversion const vpHomogeneousMatrix &depth_M_color, const vpImagePoint &from_pixel); }; END_VISP_NAMESPACE + +#endif diff --git a/modules/core/include/visp3/core/vpDebug.h b/modules/core/include/visp3/core/vpDebug.h index ef6e1eff3c..95d92041f7 100644 --- a/modules/core/include/visp3/core/vpDebug.h +++ b/modules/core/include/visp3/core/vpDebug.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -43,8 +43,8 @@ * 0 else. */ -#ifndef _vpDebug_h_ -#define _vpDebug_h_ +#ifndef VP_DEBUG_H +#define VP_DEBUG_H #include #include diff --git a/modules/core/include/visp3/core/vpDisplay.h b/modules/core/include/visp3/core/vpDisplay.h index 24b57cad3c..6e1ca8060e 100644 --- a/modules/core/include/visp3/core/vpDisplay.h +++ b/modules/core/include/visp3/core/vpDisplay.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -37,8 +37,8 @@ * with the image. */ -#ifndef _vpDisplay_h_ -#define _vpDisplay_h_ +#ifndef VP_DISPLAY_H +#define VP_DISPLAY_H #include #include diff --git a/modules/core/include/visp3/core/vpDisplayException.h b/modules/core/include/visp3/core/vpDisplayException.h index 0cc16ba313..c034be99a6 100644 --- a/modules/core/include/visp3/core/vpDisplayException.h +++ b/modules/core/include/visp3/core/vpDisplayException.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,8 +36,8 @@ * \brief error that can be emitted by the vpDisplay class and its derivatives */ -#ifndef _vpDisplayException_h_ -#define _vpDisplayException_h_ +#ifndef VP_DISPLAY_EXCEPTION_H +#define VP_DISPLAY_EXCEPTION_H #include #include diff --git a/modules/core/include/visp3/core/vpEigenConversion.h b/modules/core/include/visp3/core/vpEigenConversion.h index ac07ffc791..f64dc349be 100644 --- a/modules/core/include/visp3/core/vpEigenConversion.h +++ b/modules/core/include/visp3/core/vpEigenConversion.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,8 +31,8 @@ * ViSP <--> Eigen conversion. */ -#ifndef _vpEigenConversion_h_ -#define _vpEigenConversion_h_ +#ifndef VP_EIGEN_CONVERSION_H +#define VP_EIGEN_CONVERSION_H #include #ifdef VISP_HAVE_EIGEN3 @@ -85,10 +85,13 @@ template void visp2eigen(const VISP_NAMESPACE_ADDRESSING vpQuate template void visp2eigen(const VISP_NAMESPACE_ADDRESSING vpThetaUVector &src, Eigen::AngleAxis &dst) { + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; dst.angle() = static_cast(src.getTheta()); - dst.axis()(0) = static_cast(src.getU()[0]); - dst.axis()(1) = static_cast(src.getU()[1]); - dst.axis()(2) = static_cast(src.getU()[2]); + dst.axis()(index_0) = static_cast(src.getU()[index_0]); + dst.axis()(index_1) = static_cast(src.getU()[index_1]); + dst.axis()(index_2) = static_cast(src.getU()[index_2]); } VISP_EXPORT void visp2eigen(const VISP_NAMESPACE_ADDRESSING vpColVector &src, Eigen::VectorXd &dst); diff --git a/modules/core/include/visp3/core/vpEndian.h b/modules/core/include/visp3/core/vpEndian.h index 06202b6ffd..2e56324d68 100644 --- a/modules/core/include/visp3/core/vpEndian.h +++ b/modules/core/include/visp3/core/vpEndian.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,8 +36,8 @@ \brief Determine machine endianness and define VISP_LITTLE_ENDIAN, VISP_BIG_ENDIAN and VISP_PDP_ENDIAN macros. */ -#ifndef _vpEndian_h_ -#define _vpEndian_h_ +#ifndef VP_ENDIAN_H +#define VP_ENDIAN_H // Visual Studio 2010 or previous is missing inttypes.h #if defined(_MSC_VER) && (_MSC_VER < 1700) diff --git a/modules/core/include/visp3/core/vpException.h b/modules/core/include/visp3/core/vpException.h index 9bdb1ffbd6..c669c12bfe 100644 --- a/modules/core/include/visp3/core/vpException.h +++ b/modules/core/include/visp3/core/vpException.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,8 +36,8 @@ * \brief error that can be emitted by the vp class and its derivatives */ -#ifndef _vpException_h_ -#define _vpException_h_ +#ifndef VP_EXCEPTION_H +#define VP_EXCEPTION_H #include diff --git a/modules/core/include/visp3/core/vpExponentialMap.h b/modules/core/include/visp3/core/vpExponentialMap.h index 9ee15ffec8..c9475d17c6 100644 --- a/modules/core/include/visp3/core/vpExponentialMap.h +++ b/modules/core/include/visp3/core/vpExponentialMap.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,8 +36,8 @@ \brief Provides exponential map computation */ -#ifndef _vpExponentialMap_h_ -#define _vpExponentialMap_h_ +#ifndef VP_EXPONENTIAL_MAP_H +#define VP_EXPONENTIAL_MAP_H #include #include diff --git a/modules/core/include/visp3/core/vpFeatureDisplay.h b/modules/core/include/visp3/core/vpFeatureDisplay.h index 442f1c4fa8..ac64e73c84 100644 --- a/modules/core/include/visp3/core/vpFeatureDisplay.h +++ b/modules/core/include/visp3/core/vpFeatureDisplay.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,8 +36,8 @@ \brief interface with the image for feature display */ -#ifndef _vpFeatureDisplay_H_ -#define _vpFeatureDisplay_H_ +#ifndef VP_FEATURE_DISPLAY_H +#define VP_FEATURE_DISPLAY_H #include diff --git a/modules/core/include/visp3/core/vpForceTwistMatrix.h b/modules/core/include/visp3/core/vpForceTwistMatrix.h index c2d74b89e1..0b00589001 100644 --- a/modules/core/include/visp3/core/vpForceTwistMatrix.h +++ b/modules/core/include/visp3/core/vpForceTwistMatrix.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -32,8 +32,8 @@ * frame to an other. */ -#ifndef _vpForceTwistMatrix_h_ -#define _vpForceTwistMatrix_h_ +#ifndef VP_FORCE_TWIST_MATRIX_H +#define VP_FORCE_TWIST_MATRIX_H #include @@ -76,27 +76,27 @@ BEGIN_VISP_NAMESPACE There are different ways to initialize such a full force/torque twist matrix. The following example shows how to proceed setting the translation and rotation matrix transformations: \code -#include + #include -int main() -{ - vpTranslationVector stp(0.1, 0.2, 0.3); - vpRotationMatrix sRp( {0, 0, -1, - 0, -1, 0, - -1, 0, 0} ); - vpForceTwistMatrix sFp(stp, sRp); - std::cout << "sFp:\n" << sFp << std::endl; -} + int main() + { + vpTranslationVector stp(0.1, 0.2, 0.3); + vpRotationMatrix sRp( {0, 0, -1, + 0, -1, 0, + -1, 0, 0} ); + vpForceTwistMatrix sFp(stp, sRp); + std::cout << "sFp:\n" << sFp << std::endl; + } \endcode It produces the following printings: \code -sFp: -0 0 -1 0 0 0 -0 -1 0 0 0 0 --1 0 0 0 0 0 --0.2 0.3 0 0 0 -1 -0.1 0 -0.3 0 -1 0 -0 -0.1 0.2 -1 0 0 + sFp: + 0 0 -1 0 0 0 + 0 -1 0 0 0 0 + -1 0 0 0 0 0 + -0.2 0.3 0 0 0 -1 + 0.1 0 -0.3 0 -1 0 + 0 -0.1 0.2 -1 0 0 \endcode When the point where the velocity is expressed doesn't change, the matrix @@ -114,26 +114,26 @@ int main() To initialize such a force/torque twist matrix where translation is not taken into account you can proceed like in the following code: \code -#include + #include -int main() -{ - vpRotationMatrix sRp( {0, 0, -1, - 0, -1, 0, - -1, 0, 0} ); - vpForceTwistMatrix sFp(sRp); - std::cout << "sFp:\n" << sFp << std::endl; -} + int main() + { + vpRotationMatrix sRp( {0, 0, -1, + 0, -1, 0, + -1, 0, 0} ); + vpForceTwistMatrix sFp(sRp); + std::cout << "sFp:\n" << sFp << std::endl; + } \endcode It produces the following printings: \code -sFp: -0 0 -1 0 0 0 -0 -1 0 0 0 0 --1 0 0 0 0 0 -0 0 0 0 0 -1 -0 0 0 0 -1 0 -0 0 0 -1 0 0 + sFp: + 0 0 -1 0 0 0 + 0 -1 0 0 0 0 + -1 0 0 0 0 0 + 0 0 0 0 0 -1 + 0 0 0 0 -1 0 + 0 0 0 -1 0 0 \endcode The code belows shows for example how to convert a force/torque skew diff --git a/modules/core/include/visp3/core/vpForwardProjection.h b/modules/core/include/visp3/core/vpForwardProjection.h index 60f81141e8..5f5a275d7a 100644 --- a/modules/core/include/visp3/core/vpForwardProjection.h +++ b/modules/core/include/visp3/core/vpForwardProjection.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,8 +36,8 @@ * \brief class that defines what is a generic geometric feature */ -#ifndef _vpForwardProjection_H_ -#define _vpForwardProjection_H_ +#ifndef VP_FORWARD_PROJECTION_H +#define VP_FORWARD_PROJECTION_H #include #include diff --git a/modules/core/include/visp3/core/vpFrameGrabber.h b/modules/core/include/visp3/core/vpFrameGrabber.h index b2c62a553e..f2d96800d1 100644 --- a/modules/core/include/visp3/core/vpFrameGrabber.h +++ b/modules/core/include/visp3/core/vpFrameGrabber.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -37,8 +37,8 @@ * designed to provide a generic front end to video sources. */ -#ifndef _vpFrameGrabber_h_ -#define _vpFrameGrabber_h_ +#ifndef VP_FRAME_GRABBER_H +#define VP_FRAME_GRABBER_H #include #include diff --git a/modules/core/include/visp3/core/vpFrameGrabberException.h b/modules/core/include/visp3/core/vpFrameGrabberException.h index a0ce41e17f..30a244fd57 100644 --- a/modules/core/include/visp3/core/vpFrameGrabberException.h +++ b/modules/core/include/visp3/core/vpFrameGrabberException.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -38,8 +38,8 @@ * derivates */ -#ifndef _vpFrameGrabberException_h_ -#define _vpFrameGrabberException_h_ +#ifndef VP_FRAMEGRABBER_EXCEPTION_H +#define VP_FRAMEGRABBER_EXCEPTION_H #include #include diff --git a/modules/core/include/visp3/core/vpGEMM.h b/modules/core/include/visp3/core/vpGEMM.h index f9d385be45..99d0660918 100644 --- a/modules/core/include/visp3/core/vpGEMM.h +++ b/modules/core/include/visp3/core/vpGEMM.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,8 +31,8 @@ * Matrix generalized multiplication. */ -#ifndef _vpGEMM_h_ -#define _vpGEMM_h_ +#ifndef VP_GEMM_H +#define VP_GEMM_H #include #include diff --git a/modules/core/include/visp3/core/vpGaussianFilter.h b/modules/core/include/visp3/core/vpGaussianFilter.h index 96bef2341a..043964300b 100644 --- a/modules/core/include/visp3/core/vpGaussianFilter.h +++ b/modules/core/include/visp3/core/vpGaussianFilter.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,8 +36,8 @@ \brief Gaussian filter class */ -#ifndef _vpGaussianFilter_H_ -#define _vpGaussianFilter_H_ +#ifndef VP_GAUSSIAN_FILTER_H +#define VP_GAUSSIAN_FILTER_H #include #include diff --git a/modules/core/include/visp3/core/vpHistogram.h b/modules/core/include/visp3/core/vpHistogram.h index a9b2190c44..971bd6ce45 100644 --- a/modules/core/include/visp3/core/vpHistogram.h +++ b/modules/core/include/visp3/core/vpHistogram.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -38,8 +38,8 @@ */ -#ifndef _vpHistogram_h_ -#define _vpHistogram_h_ +#ifndef VP_HISTOGRAM_H +#define VP_HISTOGRAM_H #include @@ -49,10 +49,6 @@ #include #include -#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS -#include -#endif - #include BEGIN_VISP_NAMESPACE diff --git a/modules/core/include/visp3/core/vpHistogramPeak.h b/modules/core/include/visp3/core/vpHistogramPeak.h index e1d8e3a8cc..10b0b0eb37 100644 --- a/modules/core/include/visp3/core/vpHistogramPeak.h +++ b/modules/core/include/visp3/core/vpHistogramPeak.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -37,8 +37,8 @@ Class vpHistogramPeak defines a gray level histogram peak. */ -#ifndef _vpHistogramPeak_h_ -#define _vpHistogramPeak_h_ +#ifndef VP_HISTOGRAM_PEAK_H +#define VP_HISTOGRAM_PEAK_H #include diff --git a/modules/core/include/visp3/core/vpHistogramValey.h b/modules/core/include/visp3/core/vpHistogramValey.h index 215c6caf8c..ba92fefb10 100644 --- a/modules/core/include/visp3/core/vpHistogramValey.h +++ b/modules/core/include/visp3/core/vpHistogramValey.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -38,8 +38,8 @@ */ -#ifndef _vpHistogramValey_h_ -#define _vpHistogramValey_h_ +#ifndef VP_HISTOGRAM_VALEY_H +#define VP_HISTOGRAM_VALEY_H #include diff --git a/modules/core/include/visp3/core/vpHomogeneousMatrix.h b/modules/core/include/visp3/core/vpHomogeneousMatrix.h index 9e0e20522d..d1a6a4248b 100644 --- a/modules/core/include/visp3/core/vpHomogeneousMatrix.h +++ b/modules/core/include/visp3/core/vpHomogeneousMatrix.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,8 +36,8 @@ \brief Definition and computation on the homogeneous matrices */ -#ifndef _vpHomogeneousMatrix_h_ -#define _vpHomogeneousMatrix_h_ +#ifndef VP_HOMOGENEOUS_MATRIX_H +#define VP_HOMOGENEOUS_MATRIX_H #include #include diff --git a/modules/core/include/visp3/core/vpImage.h b/modules/core/include/visp3/core/vpImage.h index 39552fa9c1..71db936cbe 100644 --- a/modules/core/include/visp3/core/vpImage.h +++ b/modules/core/include/visp3/core/vpImage.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,11 +36,10 @@ * \brief Image handling. */ -#ifndef vpImage_H -#define vpImage_H +#ifndef VP_IMAGE_H +#define VP_IMAGE_H #include -#include #include #include #include @@ -78,11 +77,7 @@ std::ostream &operator<<(std::ostream &s, const vpImage &I); std::ostream &operator<<(std::ostream &s, const vpImage &I); std::ostream &operator<<(std::ostream &s, const vpImage &I); std::ostream &operator<<(std::ostream &s, const vpImage &I); -END_VISP_NAMESPACE - -template void swap(VISP_NAMESPACE_ADDRESSING vpImage &first, VISP_NAMESPACE_ADDRESSING vpImage &second); -BEGIN_VISP_NAMESPACE /*! \class vpImage @@ -120,16 +115,16 @@ BEGIN_VISP_NAMESPACE remark, we provide hereafter an example where the considered pixel is outside the image: -\code -unsigned int width = 320; -unsigned int height = 240; -vpImage I(height, width); // Create an 320x240 image -// Set pixel coordinates that is outside the image -unsigned int i = 100; -unsigned int j = 400; -unsigned char value; -value = I[i][j]; // Here we will get the pixel value at position (101, 80) -\endcode + \code + unsigned int width = 320; + unsigned int height = 240; + vpImage I(height, width); // Create an 320x240 image + // Set pixel coordinates that is outside the image + unsigned int i = 100; + unsigned int j = 400; + unsigned char value; + value = I[i][j]; // Here we will get the pixel value at position (101, 80) + \endcode */ template class vpImage @@ -329,7 +324,7 @@ template class vpImage friend std::ostream &operator<<(std::ostream &s, const vpImage &I); friend std::ostream &operator<<(std::ostream &s, const vpImage &I); - // Perform a look-up table transformation + // Perform a look-up table transformation void performLut(const Type(&lut)[256], unsigned int nbThreads = 1); // Returns a new image that's a quarter size of the current image @@ -344,7 +339,17 @@ template class vpImage void sub(const vpImage &A, const vpImage &B, vpImage &C) const; void subsample(unsigned int v_scale, unsigned int h_scale, vpImage &sampled) const; - friend void ::swap<>(vpImage &first, vpImage &second); + // See https://stackoverflow.com/questions/11562/how-to-overload-stdswap to understand why swap is in visp namespace + friend void swap(vpImage &first, vpImage &second) + { + using std::swap; + swap(first.bitmap, second.bitmap); + swap(first.display, second.display); + swap(first.npixels, second.npixels); + swap(first.width, second.width); + swap(first.height, second.height); + swap(first.row, second.row); + } //@} @@ -1505,7 +1510,7 @@ void vpImage::getMinMaxLoc(vpImagePoint *minLoc, vpImagePoint *maxLoc, Typ */ template vpImage &vpImage::operator=(vpImage other) { - ::swap(*this, other); + swap(*this, other); if (other.display != nullptr) { display = other.display; } @@ -1997,7 +2002,6 @@ template <> inline unsigned char vpImage::getValue(double i, doub unsigned int jround = static_cast(floor(j)); if (iround >= height || jround >= width) { - vpERROR_TRACE("Pixel outside the image"); throw(vpException(vpImageException::notInTheImage, "Pixel outside the image")); } @@ -2551,14 +2555,4 @@ template <> inline void vpImage::performLut(const vpRGBa(&lut)[256], uns } } END_VISP_NAMESPACE -template void swap(VISP_NAMESPACE_ADDRESSING vpImage &first, VISP_NAMESPACE_ADDRESSING vpImage &second) -{ - using std::swap; - swap(first.bitmap, second.bitmap); - swap(first.display, second.display); - swap(first.npixels, second.npixels); - swap(first.width, second.width); - swap(first.height, second.height); - swap(first.row, second.row); -} #endif diff --git a/modules/core/include/visp3/core/vpImageCircle.h b/modules/core/include/visp3/core/vpImageCircle.h index 997d5c4d54..15435a808f 100644 --- a/modules/core/include/visp3/core/vpImageCircle.h +++ b/modules/core/include/visp3/core/vpImageCircle.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,8 +36,8 @@ * \brief Image circle, i.e. circle in the image space. */ -#ifndef _vpImageCircle_h_ -#define _vpImageCircle_h_ +#ifndef VP_IMAGE_CIRCLE_H +#define VP_IMAGE_CIRCLE_H #include #include @@ -77,7 +77,7 @@ class VISP_EXPORT vpImageCircle * Compute the angular coverage, in terms of radians, that is contained in the Region of Interest (RoI). * \sa computeArcLengthInRoI(), computeArcLengthInRoI(const vpRect &roi) * \param[in] roi The rectangular RoI in which we want to know the number of pixels of the circle that are contained. - * \param[in] roundingTolerance The tolerance on the angle when the angle is close to a negative multiple of 2 * M_PIf. + * \param[in] roundingTolerance The tolerance on the angle when the angle is close to a negative multiple of 2 * M_PI_FLOAT. * \return Returns angular coverage of a circle in a ROI as an angle value in radians. * More precisely, it returns 2.f * M_PI for a circle that is fully visible in the RoI, or the sum of the angles * of the arc(s) that is(are) visible in the RoI. @@ -88,7 +88,7 @@ class VISP_EXPORT vpImageCircle * Compute the arc length, in terms of number of pixels, that is contained in the Region of Interest (RoI). * \sa computeAngularCoverageInRoI(), computeAngularCoverageInRoI(const vpRect &roi) * \param[in] roi The rectangular RoI in which we want to know the number of pixels of the circle that are contained. - * \param[in] roundingTolerance The tolerance on the angle when the angle is close to 2.f * M_PIf . + * \param[in] roundingTolerance The tolerance on the angle when the angle is close to 2.f * M_PI_FLOAT . * \return The number of pixels of the circle that are contained in the RoI. */ float computeArcLengthInRoI(const vpRect &roi, const float &roundingTolerance = 0.001f) const; diff --git a/modules/core/include/visp3/core/vpImageConvert.h b/modules/core/include/visp3/core/vpImageConvert.h index 4a58922a55..e2cbf19f1a 100644 --- a/modules/core/include/visp3/core/vpImageConvert.h +++ b/modules/core/include/visp3/core/vpImageConvert.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,14 +36,13 @@ \brief Convert image types */ -#ifndef _vpImageConvert_h_ -#define _vpImageConvert_h_ +#ifndef VP_IMAGE_CONVERT_H +#define VP_IMAGE_CONVERT_H #include // image #include -#include #include // color #include diff --git a/modules/core/include/visp3/core/vpImageException.h b/modules/core/include/visp3/core/vpImageException.h index aa43d9732d..d2d9dc1824 100644 --- a/modules/core/include/visp3/core/vpImageException.h +++ b/modules/core/include/visp3/core/vpImageException.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,8 +36,8 @@ * \brief error that can be emitted by the vpImage class and its derivatives */ -#ifndef _vpImageException_h_ -#define _vpImageException_h_ +#ifndef VP_IMAGE_EXCEPTION_H +#define VP_IMAGE_EXCEPTION_H #include #include diff --git a/modules/core/include/visp3/core/vpImageFilter.h b/modules/core/include/visp3/core/vpImageFilter.h index 365df9407c..fef7930875 100644 --- a/modules/core/include/visp3/core/vpImageFilter.h +++ b/modules/core/include/visp3/core/vpImageFilter.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,8 +36,8 @@ * \brief Various image filter, convolution, etc... */ -#ifndef _vpImageFilter_h_ -#define _vpImageFilter_h_ +#ifndef VP_IMAGE_FILTER_H +#define VP_IMAGE_FILTER_H #include #include @@ -1304,10 +1304,11 @@ class VISP_EXPORT vpImageFilter { const unsigned int height = I.getHeight(), width = I.getWidth(); const unsigned int stopJ = width - 3; + const unsigned int val_3 = 3; resizeAndInitializeIfNeeded(p_mask, height, width, dIx); for (unsigned int i = 0; i < height; ++i) { - for (unsigned int j = 0; j < 3; ++j) { + for (unsigned int j = 0; j < val_3; ++j) { // If a mask is used, the image is already initialized with 0s bool computeVal = (p_mask == nullptr); if (computeVal) { @@ -1393,8 +1394,8 @@ class VISP_EXPORT vpImageFilter const unsigned int height = I.getHeight(), width = I.getWidth(); const unsigned int stopI = height - 3; resizeAndInitializeIfNeeded(p_mask, height, width, dIy); - - for (unsigned int i = 0; i < 3; ++i) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { for (unsigned int j = 0; j < width; ++j) { // We have to compute the value for each pixel if we don't have a mask or for // pixels for which the mask is true otherwise @@ -1579,15 +1580,18 @@ class VISP_EXPORT vpImageFilter -100, -75, -30, -5, 0, 0, 0, 0, 0, 0, 0, 5, 30, 75, 100, 75, 30, 5, 4, 24, 60, 80, 60, 24, 4, 1, 6, 15, 20, 15, 6, 1 }; const vpArray2D smoothingKernel(3, 3); - smoothingKernel[0][0] = 1.0; - smoothingKernel[0][1] = 2.0; - smoothingKernel[0][2] = 1.0; - smoothingKernel[1][0] = 2.0; - smoothingKernel[1][1] = 4.0; - smoothingKernel[1][2] = 2.0; - smoothingKernel[2][0] = 1.0; - smoothingKernel[2][1] = 2.0; - smoothingKernel[2][2] = 1.0; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + smoothingKernel[index_0][index_0] = 1.0; + smoothingKernel[index_0][index_1] = 2.0; + smoothingKernel[index_0][index_2] = 1.0; + smoothingKernel[index_1][index_0] = 2.0; + smoothingKernel[index_1][index_1] = 4.0; + smoothingKernel[index_1][index_2] = 2.0; + smoothingKernel[index_2][index_0] = 1.0; + smoothingKernel[index_2][index_1] = 2.0; + smoothingKernel[index_2][index_2] = 1.0; if (size == 0) { throw vpException(vpException::dimensionError, "Cannot get Sobel kernel of size 0!"); diff --git a/modules/core/include/visp3/core/vpImageMorphology.h b/modules/core/include/visp3/core/vpImageMorphology.h index 9ebf79e53b..25341c0569 100644 --- a/modules/core/include/visp3/core/vpImageMorphology.h +++ b/modules/core/include/visp3/core/vpImageMorphology.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -37,8 +37,8 @@ */ -#ifndef _vpImageMorphology_h_ -#define _vpImageMorphology_h_ +#ifndef VP_IMAGE_MORPHOLOGY_H +#define VP_IMAGE_MORPHOLOGY_H #include #include diff --git a/modules/core/include/visp3/core/vpImagePoint.h b/modules/core/include/visp3/core/vpImagePoint.h index e8d34d66dc..bcee40ca41 100644 --- a/modules/core/include/visp3/core/vpImagePoint.h +++ b/modules/core/include/visp3/core/vpImagePoint.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -37,8 +37,8 @@ for image processing */ -#ifndef _vpImagePoint_h_ -#define _vpImagePoint_h_ +#ifndef VP_IMAGE_POINT_H +#define VP_IMAGE_POINT_H #include diff --git a/modules/core/include/visp3/core/vpImageTools.h b/modules/core/include/visp3/core/vpImageTools.h index 89b66be3c4..303c2e95a6 100644 --- a/modules/core/include/visp3/core/vpImageTools.h +++ b/modules/core/include/visp3/core/vpImageTools.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -38,8 +38,8 @@ the look up table, binarisation... */ -#ifndef _vpImageTools_H_ -#define _vpImageTools_H_ +#ifndef VP_IMAGE_TOOLS_H +#define VP_IMAGE_TOOLS_H #include @@ -686,7 +686,6 @@ void vpImageTools::undistort(const vpImage &I, const vpCameraParameters &c for (unsigned int i = 0; i < nthreads; ++i) { // Each thread works on a different set of data. - // vpTRACE("create thread %d", i); undistortSharedData[i].src = I.bitmap; undistortSharedData[i].dst = undistI.bitmap; undistortSharedData[i].width = I.getWidth(); @@ -700,7 +699,6 @@ void vpImageTools::undistort(const vpImage &I, const vpCameraParameters &c /* Wait on the other threads */ for (unsigned int i = 0; i < nthreads; ++i) { - // vpTRACE("join thread %d", i); threadpool[i]->join(); } @@ -1311,17 +1309,20 @@ void vpImageTools::warpImage(const vpImage &src, const vpMatrix &T, vpImag vpMatrix M = T; if (affine) { - double D = (M[0][0] * M[1][1]) - (M[0][1] * M[1][0]); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + double D = (M[index_0][index_0] * M[index_1][index_1]) - (M[index_0][index_1] * M[index_1][index_0]); D = !vpMath::nul(D, std::numeric_limits::epsilon()) ? (1.0 / D) : 0; - double A11 = M[1][1] * D, A22 = M[0][0] * D; - M[0][0] = A11; - M[0][1] *= -D; - M[1][0] *= -D; - M[1][1] = A22; - double b1 = (-M[0][0] * M[0][2]) - (M[0][1] * M[1][2]); - double b2 = (-M[1][0] * M[0][2]) - (M[1][1] * M[1][2]); - M[0][2] = b1; - M[1][2] = b2; + double A11 = M[index_1][index_1] * D, A22 = M[index_0][index_0] * D; + M[index_0][index_0] = A11; + M[index_0][index_1] *= -D; + M[index_1][index_0] *= -D; + M[index_1][index_1] = A22; + double b1 = (-M[index_0][index_0] * M[index_0][index_2]) - (M[index_0][index_1] * M[index_1][index_2]); + double b2 = (-M[index_1][index_0] * M[index_0][index_2]) - (M[index_1][index_1] * M[index_1][index_2]); + M[index_0][index_2] = b1; + M[index_1][index_2] = b2; } else { M = T.inverseByLU(); @@ -1351,16 +1352,18 @@ void vpImageTools::warpNN(const vpImage &src, const vpMatrix &T, vpImage(precision); - - int32_t a0_i32 = static_cast(T[0][0] * precision); - int32_t a1_i32 = static_cast(T[0][1] * precision); - int32_t a2_i32 = static_cast(T[0][2] * precision); - int32_t a3_i32 = static_cast(T[1][0] * precision); - int32_t a4_i32 = static_cast(T[1][1] * precision); - int32_t a5_i32 = static_cast(T[1][2] * precision); - int32_t a6_i32 = T.getRows() == 3 ? static_cast(T[2][0] * precision) : 0; - int32_t a7_i32 = T.getRows() == 3 ? static_cast(T[2][1] * precision) : 0; - int32_t a8_i32 = T.getRows() == 3 ? static_cast(T[2][2] * precision) : 1; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + int32_t a0_i32 = static_cast(T[index_0][index_0] * precision); + int32_t a1_i32 = static_cast(T[index_0][index_1] * precision); + int32_t a2_i32 = static_cast(T[index_0][index_2] * precision); + int32_t a3_i32 = static_cast(T[index_1][index_0] * precision); + int32_t a4_i32 = static_cast(T[index_1][index_1] * precision); + int32_t a5_i32 = static_cast(T[index_1][index_2] * precision); + int32_t a6_i32 = T.getRows() == 3 ? static_cast(T[index_2][index_0] * precision) : 0; + int32_t a7_i32 = T.getRows() == 3 ? static_cast(T[index_2][index_1] * precision) : 0; + int32_t a8_i32 = T.getRows() == 3 ? static_cast(T[index_2][index_2] * precision) : 1; int32_t height_1_i32 = static_cast((src.getHeight() - 1) * precision) + 0x8000; int32_t width_1_i32 = static_cast((src.getWidth() - 1) * precision) + 0x8000; @@ -1426,15 +1429,18 @@ void vpImageTools::warpNN(const vpImage &src, const vpMatrix &T, vpImage &src, const vpMatrix &T, vpIma const float precision_1 = 1 / static_cast(precision); const uint64_t precision2 = 1ULL << (2 * nbits); const float precision_2 = 1 / static_cast(precision2); - - int64_t a0_i64 = static_cast(T[0][0] * precision); - int64_t a1_i64 = static_cast(T[0][1] * precision); - int64_t a2_i64 = static_cast(T[0][2] * precision); - int64_t a3_i64 = static_cast(T[1][0] * precision); - int64_t a4_i64 = static_cast(T[1][1] * precision); - int64_t a5_i64 = static_cast(T[1][2] * precision); - int64_t a6_i64 = T.getRows() == 3 ? static_cast(T[2][0] * precision) : 0; - int64_t a7_i64 = T.getRows() == 3 ? static_cast(T[2][1] * precision) : 0; - int64_t a8_i64 = T.getRows() == 3 ? static_cast(T[2][2] * precision) : 1; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + + int64_t a0_i64 = static_cast(T[index_0][index_0] * precision); + int64_t a1_i64 = static_cast(T[index_0][index_1] * precision); + int64_t a2_i64 = static_cast(T[index_0][index_2] * precision); + int64_t a3_i64 = static_cast(T[index_1][index_0] * precision); + int64_t a4_i64 = static_cast(T[index_1][index_1] * precision); + int64_t a5_i64 = static_cast(T[index_1][index_2] * precision); + int64_t a6_i64 = T.getRows() == 3 ? static_cast(T[index_2][index_0] * precision) : 0; + int64_t a7_i64 = T.getRows() == 3 ? static_cast(T[index_2][index_1] * precision) : 0; + int64_t a8_i64 = T.getRows() == 3 ? static_cast(T[index_2][index_2] * precision) : 1; int64_t height_i64 = static_cast(src.getHeight() * precision); int64_t width_i64 = static_cast(src.getWidth() * precision); @@ -1603,15 +1612,18 @@ void vpImageTools::warpLinear(const vpImage &src, const vpMatrix &T, vpIma } } else { - double a0 = T[0][0]; - double a1 = T[0][1]; - double a2 = T[0][2]; - double a3 = T[1][0]; - double a4 = T[1][1]; - double a5 = T[1][2]; - double a6 = affine ? 0.0 : T[2][0]; - double a7 = affine ? 0.0 : T[2][1]; - double a8 = affine ? 1.0 : T[2][2]; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + double a0 = T[index_0][index_0]; + double a1 = T[index_0][index_1]; + double a2 = T[index_0][index_2]; + double a3 = T[index_1][index_0]; + double a4 = T[index_1][index_1]; + double a5 = T[index_1][index_2]; + double a6 = affine ? 0.0 : T[index_2][index_0]; + double a7 = affine ? 0.0 : T[index_2][index_1]; + double a8 = affine ? 1.0 : T[index_2][index_2]; unsigned int dst_height = dst.getHeight(); unsigned int dst_width = dst.getWidth(); @@ -1631,38 +1643,39 @@ void vpImageTools::warpLinear(const vpImage &src, const vpMatrix &T, vpIma int x_lower = static_cast(x); int y_lower = static_cast(y); - + bool stop_for_loop = false; if ((y_lower >= src_height) || (x_lower >= src_width) || (y < 0) || (x < 0)) { - continue; - } - - double s = x - x_lower; - double t = y - y_lower; - - if ((y_lower < (src_height - 1)) && (x_lower < (src_width - 1))) { - const double val00 = static_cast(src[y_lower][x_lower]); - const double val01 = static_cast(src[y_lower][x_lower + 1]); - const double val10 = static_cast(src[y_lower + 1][x_lower]); - const double val11 = static_cast(src[y_lower + 1][x_lower + 1]); - const double col0 = lerp(val00, val01, s); - const double col1 = lerp(val10, val11, s); - const double interp = lerp(col0, col1, t); - dst[i][j] = vpMath::saturate(interp); - } - else if (y_lower < (src_height - 1)) { - const double val00 = static_cast(src[y_lower][x_lower]); - const double val10 = static_cast(src[y_lower + 1][x_lower]); - const double interp = lerp(val00, val10, t); - dst[i][j] = vpMath::saturate(interp); + stop_for_loop = true; } - else if (x_lower < (src_width - 1)) { - const double val00 = static_cast(src[y_lower][x_lower]); - const double val01 = static_cast(src[y_lower][x_lower + 1]); - const double interp = lerp(val00, val01, s); - dst[i][j] = vpMath::saturate(interp); - } - else { - dst[i][j] = src[y_lower][x_lower]; + if (!stop_for_loop) { + double s = x - x_lower; + double t = y - y_lower; + + if ((y_lower < (src_height - 1)) && (x_lower < (src_width - 1))) { + const double val00 = static_cast(src[y_lower][x_lower]); + const double val01 = static_cast(src[y_lower][x_lower + 1]); + const double val10 = static_cast(src[y_lower + 1][x_lower]); + const double val11 = static_cast(src[y_lower + 1][x_lower + 1]); + const double col0 = lerp(val00, val01, s); + const double col1 = lerp(val10, val11, s); + const double interp = lerp(col0, col1, t); + dst[i][j] = vpMath::saturate(interp); + } + else if (y_lower < (src_height - 1)) { + const double val00 = static_cast(src[y_lower][x_lower]); + const double val10 = static_cast(src[y_lower + 1][x_lower]); + const double interp = lerp(val00, val10, t); + dst[i][j] = vpMath::saturate(interp); + } + else if (x_lower < (src_width - 1)) { + const double val00 = static_cast(src[y_lower][x_lower]); + const double val01 = static_cast(src[y_lower][x_lower + 1]); + const double interp = lerp(val00, val01, s); + dst[i][j] = vpMath::saturate(interp); + } + else { + dst[i][j] = src[y_lower][x_lower]; + } } } } @@ -1673,6 +1686,9 @@ template <> inline void vpImageTools::warpLinear(const vpImage &src, const vpMatrix &T, vpImage &dst, bool affine, bool centerCorner, bool fixedPoint) { + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; if (fixedPoint && (!centerCorner)) { const int nbits = 16; const int64_t precision = 1 << nbits; @@ -1680,14 +1696,14 @@ inline void vpImageTools::warpLinear(const vpImage &src, const vpMatrix const int64_t precision2 = 1ULL << (2 * nbits); const float precision_2 = 1 / static_cast(precision2); - int64_t a0_i64 = static_cast(T[0][0] * precision); - int64_t a1_i64 = static_cast(T[0][1] * precision); - int64_t a2_i64 = static_cast(T[0][2] * precision); - int64_t a3_i64 = static_cast(T[1][0] * precision); - int64_t a4_i64 = static_cast(T[1][1] * precision); - int64_t a5_i64 = static_cast(T[1][2] * precision); - int64_t a6_i64 = T.getRows() == 3 ? static_cast(T[2][0] * precision) : 0; - int64_t a7_i64 = T.getRows() == 3 ? static_cast(T[2][1] * precision) : 0; + int64_t a0_i64 = static_cast(T[index_0][index_0] * precision); + int64_t a1_i64 = static_cast(T[index_0][index_1] * precision); + int64_t a2_i64 = static_cast(T[index_0][index_2] * precision); + int64_t a3_i64 = static_cast(T[index_1][index_0] * precision); + int64_t a4_i64 = static_cast(T[index_1][index_1] * precision); + int64_t a5_i64 = static_cast(T[index_1][index_2] * precision); + int64_t a6_i64 = T.getRows() == 3 ? static_cast(T[index_2][index_0] * precision) : 0; + int64_t a7_i64 = T.getRows() == 3 ? static_cast(T[index_2][index_1] * precision) : 0; int64_t a8_i64 = precision; int64_t height_i64 = static_cast(src.getHeight() * precision); @@ -1858,15 +1874,15 @@ inline void vpImageTools::warpLinear(const vpImage &src, const vpMatrix } } else { - double a0 = T[0][0]; - double a1 = T[0][1]; - double a2 = T[0][2]; - double a3 = T[1][0]; - double a4 = T[1][1]; - double a5 = T[1][2]; - double a6 = affine ? 0.0 : T[2][0]; - double a7 = affine ? 0.0 : T[2][1]; - double a8 = affine ? 1.0 : T[2][2]; + double a0 = T[index_0][index_0]; + double a1 = T[index_0][index_1]; + double a2 = T[index_0][index_2]; + double a3 = T[index_1][index_0]; + double a4 = T[index_1][index_1]; + double a5 = T[index_1][index_2]; + double a6 = affine ? 0.0 : T[index_2][index_0]; + double a7 = affine ? 0.0 : T[index_2][index_1]; + double a8 = affine ? 1.0 : T[index_2][index_2]; unsigned int dst_height = dst.getHeight(); unsigned int dst_width = dst.getWidth(); @@ -1884,55 +1900,57 @@ inline void vpImageTools::warpLinear(const vpImage &src, const vpMatrix int x_lower = static_cast(x); int y_lower = static_cast(y); + bool stop_for_loop = false; if ((y_lower >= src_height) || (x_lower >= src_width) || (y < 0) || (x < 0)) { - continue; + stop_for_loop = true; } - - double s = x - x_lower; - double t = y - y_lower; - - if ((y_lower < (src_height - 1)) && (x_lower < (src_width - 1))) { - const vpRGBa val00 = src[y_lower][x_lower]; - const vpRGBa val01 = src[y_lower][x_lower + 1]; - const vpRGBa val10 = src[y_lower + 1][x_lower]; - const vpRGBa val11 = src[y_lower + 1][x_lower + 1]; - const double colR0 = lerp(val00.R, val01.R, s); - const double colR1 = lerp(val10.R, val11.R, s); - const double interpR = lerp(colR0, colR1, t); - - const double colG0 = lerp(val00.G, val01.G, s); - const double colG1 = lerp(val10.G, val11.G, s); - const double interpG = lerp(colG0, colG1, t); - - const double colB0 = lerp(val00.B, val01.B, s); - const double colB1 = lerp(val10.B, val11.B, s); - const double interpB = lerp(colB0, colB1, t); - - dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), - vpMath::saturate(interpB), 255); - } - else if (y_lower < (src_height - 1)) { - const vpRGBa val00 = src[y_lower][x_lower]; - const vpRGBa val10 = src[y_lower + 1][x_lower]; - const double interpR = lerp(val00.R, val10.R, t); - const double interpG = lerp(val00.G, val10.G, t); - const double interpB = lerp(val00.B, val10.B, t); - - dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), - vpMath::saturate(interpB), 255); - } - else if (x_lower < (src_width - 1)) { - const vpRGBa val00 = src[y_lower][x_lower]; - const vpRGBa val01 = src[y_lower][x_lower + 1]; - const double interpR = lerp(val00.R, val01.R, s); - const double interpG = lerp(val00.G, val01.G, s); - const double interpB = lerp(val00.B, val01.B, s); - - dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), - vpMath::saturate(interpB), 255); - } - else { - dst[i][j] = src[y_lower][x_lower]; + if (!stop_for_loop) { + double s = x - x_lower; + double t = y - y_lower; + + if ((y_lower < (src_height - 1)) && (x_lower < (src_width - 1))) { + const vpRGBa val00 = src[y_lower][x_lower]; + const vpRGBa val01 = src[y_lower][x_lower + 1]; + const vpRGBa val10 = src[y_lower + 1][x_lower]; + const vpRGBa val11 = src[y_lower + 1][x_lower + 1]; + const double colR0 = lerp(val00.R, val01.R, s); + const double colR1 = lerp(val10.R, val11.R, s); + const double interpR = lerp(colR0, colR1, t); + + const double colG0 = lerp(val00.G, val01.G, s); + const double colG1 = lerp(val10.G, val11.G, s); + const double interpG = lerp(colG0, colG1, t); + + const double colB0 = lerp(val00.B, val01.B, s); + const double colB1 = lerp(val10.B, val11.B, s); + const double interpB = lerp(colB0, colB1, t); + + dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), + vpMath::saturate(interpB), 255); + } + else if (y_lower < (src_height - 1)) { + const vpRGBa val00 = src[y_lower][x_lower]; + const vpRGBa val10 = src[y_lower + 1][x_lower]; + const double interpR = lerp(val00.R, val10.R, t); + const double interpG = lerp(val00.G, val10.G, t); + const double interpB = lerp(val00.B, val10.B, t); + + dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), + vpMath::saturate(interpB), 255); + } + else if (x_lower < (src_width - 1)) { + const vpRGBa val00 = src[y_lower][x_lower]; + const vpRGBa val01 = src[y_lower][x_lower + 1]; + const double interpR = lerp(val00.R, val01.R, s); + const double interpG = lerp(val00.G, val01.G, s); + const double interpB = lerp(val00.B, val01.B, s); + + dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), + vpMath::saturate(interpB), 255); + } + else { + dst[i][j] = src[y_lower][x_lower]; + } } } } diff --git a/modules/core/include/visp3/core/vpIoException.h b/modules/core/include/visp3/core/vpIoException.h index 100721f44f..8482bd1fab 100644 --- a/modules/core/include/visp3/core/vpIoException.h +++ b/modules/core/include/visp3/core/vpIoException.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,8 +36,8 @@ * \brief Error that can be emitted by the vpIoTools class and its derivatives. */ -#ifndef _vpIoException_h_ -#define _vpIoException_h_ +#ifndef VP_IO_EXCEPTION_H +#define VP_IO_EXCEPTION_H #include #include diff --git a/modules/core/include/visp3/core/vpIoTools.h b/modules/core/include/visp3/core/vpIoTools.h index e7eb2017c3..6eff355ae4 100644 --- a/modules/core/include/visp3/core/vpIoTools.h +++ b/modules/core/include/visp3/core/vpIoTools.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,8 +36,8 @@ * \brief File and directories basic tools. */ -#ifndef _vpIoTools_h_ -#define _vpIoTools_h_ +#ifndef VP_IO_TOOLS_H +#define VP_IO_TOOLS_H #include diff --git a/modules/core/include/visp3/core/vpJsonParsing.h b/modules/core/include/visp3/core/vpJsonParsing.h index 7c4a163f43..e70baeab68 100644 --- a/modules/core/include/visp3/core/vpJsonParsing.h +++ b/modules/core/include/visp3/core/vpJsonParsing.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,8 +31,8 @@ * JSON parsing helpers. */ -#ifndef _vpJsonParsing_h_ -#define _vpJsonParsing_h_ +#ifndef VP_JSON_PARSING_H +#define VP_JSON_PARSING_H #include diff --git a/modules/core/include/visp3/core/vpLine.h b/modules/core/include/visp3/core/vpLine.h index 61ea644b47..fee8d08505 100644 --- a/modules/core/include/visp3/core/vpLine.h +++ b/modules/core/include/visp3/core/vpLine.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,8 +36,8 @@ * \brief class that defines what is a line */ -#ifndef _vpLine_H_ -#define _vpLine_H_ +#ifndef VP_LINE_H +#define VP_LINE_H #include #include diff --git a/modules/core/include/visp3/core/vpList.h b/modules/core/include/visp3/core/vpList.h index 854f5ce6dc..f62bff5cc8 100644 --- a/modules/core/include/visp3/core/vpList.h +++ b/modules/core/include/visp3/core/vpList.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,8 +36,8 @@ * \brief Definition of the list management class */ -#ifndef _VP_LIST_H_ -#define _VP_LIST_H_ +#ifndef VP_LIST_H +#define VP_LIST_H #include #include diff --git a/modules/core/include/visp3/core/vpMath.h b/modules/core/include/visp3/core/vpMath.h index 5bee7e898d..c4561b5c0b 100644 --- a/modules/core/include/visp3/core/vpMath.h +++ b/modules/core/include/visp3/core/vpMath.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -37,8 +37,8 @@ * the C mathematics library (math.h) */ -#ifndef _vpMATH_H_ -#define _vpMATH_H_ +#ifndef VP_MATH_H +#define VP_MATH_H #include @@ -77,16 +77,16 @@ #endif -#ifndef M_PIf -#define M_PIf 3.14159265358979323846f +#ifndef M_PI_FLOAT +#define M_PI_FLOAT 3.14159265358979323846f #endif -#ifndef M_PI_2f -#define M_PI_2f (M_PIf / 2.0f) +#ifndef M_PI_2_FLOAT +#define M_PI_2_FLOAT (M_PI_FLOAT / 2.0f) #endif -#ifndef M_PI_4f -#define M_PI_4f (M_PIf / 4.0f) +#ifndef M_PI_4_FLOAT +#define M_PI_4_FLOAT (M_PI_FLOAT / 4.0f) #endif #include @@ -139,11 +139,11 @@ class VISP_EXPORT vpMath static float getAngleBetweenMinPiAndPi(const float &theta) { float theta1 = theta; - if (theta1 > M_PIf) { - theta1 -= 2.0f * M_PIf; + if (theta1 > M_PI_FLOAT) { + theta1 -= 2.0f * M_PI_FLOAT; } - else if (theta1 <= -M_PIf) { - theta1 += 2.0f * M_PIf; + else if (theta1 <= -M_PI_FLOAT) { + theta1 += 2.0f * M_PI_FLOAT; } return theta1; } diff --git a/modules/core/include/visp3/core/vpMatrix.h b/modules/core/include/visp3/core/vpMatrix.h index 4c985d1c5e..55a7b41d1a 100644 --- a/modules/core/include/visp3/core/vpMatrix.h +++ b/modules/core/include/visp3/core/vpMatrix.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -38,12 +38,12 @@ these matrices. */ -#ifndef _vpMatrix_H_ -#define _vpMatrix_H_ +#ifndef VP_MATRIX_H +#define VP_MATRIX_H #include -BEGIN_VISP_NAMESPACE +BEGIN_VISP_NAMESPACE class vpRowVector; class vpColVector; class vpTranslationVector; @@ -92,56 +92,56 @@ BEGIN_VISP_NAMESPACE The code below shows how to create a 2-by-3 matrix of doubles, set the element values and access them: \code -#include > &) \code -int main() -{ - vpMatrix M; - M = { {-1, -2, -3}, {4, 5.5, 6.0f} }; -} + int main() + { + vpMatrix M; + M = { {-1, -2, -3}, {4, 5.5, 6.0f} }; + } \endcode \sa vpArray2D, vpRowVector, vpColVector, vpHomogeneousMatrix, diff --git a/modules/core/include/visp3/core/vpMatrixException.h b/modules/core/include/visp3/core/vpMatrixException.h index c8d084b7f9..3aa5d14833 100644 --- a/modules/core/include/visp3/core/vpMatrixException.h +++ b/modules/core/include/visp3/core/vpMatrixException.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,8 +31,8 @@ * Exceptions that can be emitted by the vpMatrix class and its derivatives. */ -#ifndef _vpMatrixException_h_ -#define _vpMatrixException_h_ +#ifndef VP_MATRIX_EXCEPTION_H +#define VP_MATRIX_EXCEPTION_H #include #include diff --git a/modules/core/include/visp3/core/vpMeterPixelConversion.h b/modules/core/include/visp3/core/vpMeterPixelConversion.h index 5aae2c2fc4..eeee75f8d4 100644 --- a/modules/core/include/visp3/core/vpMeterPixelConversion.h +++ b/modules/core/include/visp3/core/vpMeterPixelConversion.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,8 +36,8 @@ \brief Meter to pixel conversion. */ -#ifndef _vpMeterPixelConversion_h_ -#define _vpMeterPixelConversion_h_ +#ifndef VP_METER_PIXEL_CONVERSION_H +#define VP_METER_PIXEL_CONVERSION_H #include #include @@ -272,13 +272,17 @@ class VISP_EXPORT vpMeterPixelConversion { double r = sqrt(vpMath::sqr(x) + vpMath::sqr(y)); double theta = atan(r); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; std::vector k = cam.getKannalaBrandtDistortionCoefficients(); double theta2 = theta * theta, theta3 = theta2 * theta, theta4 = theta2 * theta2, theta5 = theta4 * theta, theta6 = theta3 * theta3, theta7 = theta6 * theta, theta8 = theta4 * theta4, theta9 = theta8 * theta; - double r_d = theta + (k[0] * theta3) + (k[1] * theta5) + (k[2] * theta7) + (k[3] * theta9); + double r_d = theta + (k[index_0] * theta3) + (k[index_1] * theta5) + (k[index_2] * theta7) + (k[index_3] * theta9); double scale = (std::fabs(r) < std::numeric_limits::epsilon()) ? 1.0 : (r_d / r); @@ -315,13 +319,17 @@ class VISP_EXPORT vpMeterPixelConversion { double r = sqrt(vpMath::sqr(x) + vpMath::sqr(y)); double theta = atan(r); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; std::vector k = cam.getKannalaBrandtDistortionCoefficients(); double theta2 = theta * theta, theta3 = theta2 * theta, theta4 = theta2 * theta2, theta5 = theta4 * theta, theta6 = theta3 * theta3, theta7 = theta6 * theta, theta8 = theta4 * theta4, theta9 = theta8 * theta; - double r_d = theta + (k[0] * theta3) + (k[1] * theta5) + (k[2] * theta7) + (k[3] * theta9); + double r_d = theta + (k[index_0] * theta3) + (k[index_1] * theta5) + (k[index_2] * theta7) + (k[index_3] * theta9); double scale = (std::fabs(r) < std::numeric_limits::epsilon()) ? 1.0 : (r_d / r); diff --git a/modules/core/include/visp3/core/vpMouseButton.h b/modules/core/include/visp3/core/vpMouseButton.h index 5c72ba5841..1d382b71a2 100644 --- a/modules/core/include/visp3/core/vpMouseButton.h +++ b/modules/core/include/visp3/core/vpMouseButton.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,8 +31,8 @@ * Color definition. */ -#ifndef _vpMouseButton_h_ -#define _vpMouseButton_h_ +#ifndef VP_MOUSE_BUTTON_H +#define VP_MOUSE_BUTTON_H #include diff --git a/modules/core/include/visp3/core/vpNullptrEmulated.h b/modules/core/include/visp3/core/vpNullptrEmulated.h index 687eb5f3f6..8b4031b13f 100644 --- a/modules/core/include/visp3/core/vpNullptrEmulated.h +++ b/modules/core/include/visp3/core/vpNullptrEmulated.h @@ -28,8 +28,8 @@ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. */ -#ifndef _vpNullptrEmulated_h_ -#define _vpNullptrEmulated_h_ +#ifndef VP_NULLPTR_EMULATED_H +#define VP_NULLPTR_EMULATED_H #include diff --git a/modules/core/include/visp3/core/vpPixelMeterConversion.h b/modules/core/include/visp3/core/vpPixelMeterConversion.h index c3cacbc985..273333d118 100644 --- a/modules/core/include/visp3/core/vpPixelMeterConversion.h +++ b/modules/core/include/visp3/core/vpPixelMeterConversion.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,11 +36,10 @@ \brief pixel to meter conversion */ -#ifndef _vpPixelMeterConversion_h_ -#define _vpPixelMeterConversion_h_ +#ifndef VP_PIXEL_METER_CONVERSION_H +#define VP_PIXEL_METER_CONVERSION_H #include -#include #include #include #include @@ -276,6 +275,16 @@ class VISP_EXPORT vpPixelMeterConversion double x_d = (u - cam.m_u0) / cam.m_px, y_d = (v - cam.m_v0) / cam.m_py; double scale = 1.0; double r_d = sqrt(vpMath::sqr(x_d) + vpMath::sqr(y_d)); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int val_1 = 1; + const unsigned int val_3 = 3; + const unsigned int val_5 = 5; + const unsigned int val_7 = 7; + const unsigned int val_9 = 9; + const unsigned int val_10 = 10; r_d = std::min(std::max(-M_PI, r_d), M_PI); // FOV restricted to 180degrees. @@ -287,15 +296,20 @@ class VISP_EXPORT vpPixelMeterConversion // compensate distortion iteratively double theta = r_d; - for (int j = 0; j < 10; ++j) { - double theta2 = theta * theta, theta4 = theta2 * theta2, theta6 = theta4 * theta2, theta8 = theta6 * theta2; - double k0_theta2 = k[0] * theta2, k1_theta4 = k[1] * theta4, k2_theta6 = k[2] * theta6, - k3_theta8 = k[3] * theta8; + for (unsigned int j = 0; j < val_10; ++j) { + double theta2 = theta * theta; + double theta4 = theta2 * theta2; + double theta6 = theta4 * theta2; + double theta8 = theta6 * theta2; + double k0_theta2 = k[index_0] * theta2; + double k1_theta4 = k[index_1] * theta4; + double k2_theta6 = k[index_2] * theta6, + k3_theta8 = k[index_3] * theta8; /* // new_theta = theta - theta_fix, theta_fix = f0(theta) / f0'(theta) */ - double theta_fix = (theta * (1 + k0_theta2 + k1_theta4 + k2_theta6 + k3_theta8) - r_d) / - (1 + 3 * k0_theta2 + 5 * k1_theta4 + 7 * k2_theta6 + 9 * k3_theta8); + double theta_fix = (theta * (val_1 + k0_theta2 + k1_theta4 + k2_theta6 + k3_theta8) - r_d) / + (val_1 + val_3 * k0_theta2 + val_5 * k1_theta4 + val_7 * k2_theta6 + val_9 * k3_theta8); theta = theta - theta_fix; if (fabs(theta_fix) < EPS) { break; @@ -334,6 +348,16 @@ class VISP_EXPORT vpPixelMeterConversion double x_d = (iP.get_u() - cam.m_u0) / cam.m_px, y_d = (iP.get_v() - cam.m_v0) / cam.m_py; double scale = 1.0; double r_d = sqrt(vpMath::sqr(x_d) + vpMath::sqr(y_d)); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int val_1 = 1; + const unsigned int val_3 = 3; + const unsigned int val_5 = 5; + const unsigned int val_7 = 7; + const unsigned int val_9 = 9; + const unsigned int val_10 = 10; r_d = std::min(std::max(-M_PI, r_d), M_PI); // FOV restricted to 180degrees. @@ -345,18 +369,24 @@ class VISP_EXPORT vpPixelMeterConversion // compensate distortion iteratively double theta = r_d; - for (int j = 0; j < 10; ++j) { - double theta2 = theta * theta, theta4 = theta2 * theta2, theta6 = theta4 * theta2, theta8 = theta6 * theta2; - double k0_theta2 = k[0] * theta2, k1_theta4 = k[1] * theta4, k2_theta6 = k[2] * theta6, - k3_theta8 = k[3] * theta8; + for (unsigned int j = 0; j < val_10; ++j) { + double theta2 = theta * theta; + double theta4 = theta2 * theta2; + double theta6 = theta4 * theta2; + double theta8 = theta6 * theta2; + double k0_theta2 = k[index_0] * theta2; + double k1_theta4 = k[index_1] * theta4; + double k2_theta6 = k[index_2] * theta6; + double k3_theta8 = k[index_3] * theta8; /* // new_theta = theta - theta_fix, theta_fix = f0(theta) / f0'(theta) */ - double theta_fix = ((theta * (1 + k0_theta2 + k1_theta4 + k2_theta6 + k3_theta8)) - r_d) / - (1 + (3 * k0_theta2) + (5 * k1_theta4) + (7 * k2_theta6) + (9 * k3_theta8)); + double theta_fix = ((theta * (val_1 + k0_theta2 + k1_theta4 + k2_theta6 + k3_theta8)) - r_d) / + (val_1 + (val_3 * k0_theta2) + (val_5 * k1_theta4) + (val_7 * k2_theta6) + (val_9 * k3_theta8)); theta = theta - theta_fix; - if (fabs(theta_fix) < EPS) + if (fabs(theta_fix) < EPS) { break; + } } scale = std::tan(theta) / r_d; // Scale of norm of (x,y) and (x_d, y_d) diff --git a/modules/core/include/visp3/core/vpPlane.h b/modules/core/include/visp3/core/vpPlane.h index 9741cd713c..d57f03039a 100644 --- a/modules/core/include/visp3/core/vpPlane.h +++ b/modules/core/include/visp3/core/vpPlane.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,8 +31,8 @@ * Plane geometrical structure. */ -#ifndef _vpPlane_h_ -#define _vpPlane_h_ +#ifndef VP_PLANE_H +#define VP_PLANE_H #include #include @@ -55,14 +55,11 @@ BEGIN_VISP_NAMESPACE */ class VISP_EXPORT vpPlane { - #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS // for backward compatibility public: -#else -private: -#endif double A, B, C, D; +#endif public: typedef enum { object_frame, camera_frame } vpPlaneFrame; @@ -116,14 +113,19 @@ class VISP_EXPORT vpPlane */ inline vpColVector getABCD() const { + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; vpColVector n(4); - n[0] = A; - n[1] = B; - n[2] = C; - n[3] = D; + n[index_0] = A; + n[index_1] = B; + n[index_2] = C; + n[index_3] = D; return n; } + /*! \warning This method is provided for compatibility with the @@ -136,11 +138,15 @@ class VISP_EXPORT vpPlane */ inline vpColVector abcd() const { + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; vpColVector n(4); - n[0] = A; - n[1] = B; - n[2] = C; - n[3] = D; + n[index_0] = A; + n[index_1] = B; + n[index_2] = C; + n[index_3] = D; return n; } @@ -157,6 +163,11 @@ class VISP_EXPORT vpPlane double getIntersection(const vpColVector &M1, vpColVector &H) const; void changeFrame(const vpHomogeneousMatrix &cMo); + +#ifndef VISP_BUILD_DEPRECATED_FUNCTIONS +private: + double A, B, C, D; +#endif }; END_VISP_NAMESPACE #endif diff --git a/modules/core/include/visp3/core/vpPoint.h b/modules/core/include/visp3/core/vpPoint.h index 2232816dec..8edb243ea8 100644 --- a/modules/core/include/visp3/core/vpPoint.h +++ b/modules/core/include/visp3/core/vpPoint.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,8 +36,8 @@ \brief class that defines what is a point */ -#ifndef _vpPoint_H_ -#define _vpPoint_H_ +#ifndef VP_POINT_H +#define VP_POINT_H #include #include diff --git a/modules/core/include/visp3/core/vpPolygon.h b/modules/core/include/visp3/core/vpPolygon.h index d0dfa15be2..48b7f20a5c 100644 --- a/modules/core/include/visp3/core/vpPolygon.h +++ b/modules/core/include/visp3/core/vpPolygon.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,8 +31,8 @@ * Defines a generic 2D polygon. */ -#ifndef _vpPolygon_h_ -#define _vpPolygon_h_ +#ifndef VP_POLYGON_H +#define VP_POLYGON_H #include #include @@ -58,46 +58,45 @@ BEGIN_VISP_NAMESPACE \f$, \f$ (1,0) \f$ and \f$ (0,1) \f$. The code bellow shows how to manipulate a polygon. -\code -#include + \code + #include -#include + #include -int main() -{ - std::vector corners; - - // Initialize the corners vector with 4 points - corners.push_back( vpImagePoint( 50, 100) ); - corners.push_back( vpImagePoint( 50, 300) ); - corners.push_back( vpImagePoint(200, 300) ); - corners.push_back( vpImagePoint(200, 100) ); - - // Initialize a polygon with the corners - vpPolygon polygon(corners); - - // Get the polygon bounding box - vpRect bbox = polygon.getBoundingBox(); - std::cout << "Bounding box: " << bbox.getTopLeft() << " to " - << bbox.getBottomRight() << std::endl; - - // Get the polygon surface and center - std::cout << "Area: " << polygon.getArea() << std::endl; - std::cout << "Center: " << polygon.getCenter() << std::endl; - - // Check if a point is inside the polygon - vpImagePoint ip(550, 200); - std::cout << "The point " << ip << " is " - << (polygon.isInside(ip) ? "inside":"outside") - << " the polygon" << std::endl; - - return 0; -} -\endcode + int main() + { + std::vector corners; + + // Initialize the corners vector with 4 points + corners.push_back( vpImagePoint( 50, 100) ); + corners.push_back( vpImagePoint( 50, 300) ); + corners.push_back( vpImagePoint(200, 300) ); + corners.push_back( vpImagePoint(200, 100) ); + + // Initialize a polygon with the corners + vpPolygon polygon(corners); + + // Get the polygon bounding box + vpRect bbox = polygon.getBoundingBox(); + std::cout << "Bounding box: " << bbox.getTopLeft() << " to " + << bbox.getBottomRight() << std::endl; + + // Get the polygon surface and center + std::cout << "Area: " << polygon.getArea() << std::endl; + std::cout << "Center: " << polygon.getCenter() << std::endl; + + // Check if a point is inside the polygon + vpImagePoint ip(550, 200); + std::cout << "The point " << ip << " is " + << (polygon.isInside(ip) ? "inside":"outside") + << " the polygon" << std::endl; + + return 0; + } + \endcode */ class VISP_EXPORT vpPolygon { - public: enum PointInPolygonMethod { @@ -166,6 +165,12 @@ class VISP_EXPORT vpPolygon */ inline vpRect getBoundingBox() const { return _bbox; } + //################### + // Static Functions + //################### + static bool isInside(const std::vector &roi, const double &i, const double &j, + const PointInPolygonMethod &method = PnPolyRayCasting); + protected: void init(const std::vector &corners); void init(const std::list &corners); @@ -194,14 +199,6 @@ class VISP_EXPORT vpPolygon std::vector m_PnPolyConstants; std::vector m_PnPolyMultiples; - - //################### - // Static Functions - //################### - -public: - static bool isInside(const std::vector &roi, const double &i, const double &j, - const PointInPolygonMethod &method = PnPolyRayCasting); }; END_VISP_NAMESPACE #endif diff --git a/modules/core/include/visp3/core/vpPoseVector.h b/modules/core/include/visp3/core/vpPoseVector.h index 35fca304c7..80bceaae4c 100644 --- a/modules/core/include/visp3/core/vpPoseVector.h +++ b/modules/core/include/visp3/core/vpPoseVector.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -40,8 +40,8 @@ translation vector. */ -#ifndef _vpPOSEVECTOR_H_ -#define _vpPOSEVECTOR_H_ +#ifndef VP_POSE_VECTOR_H +#define VP_POSE_VECTOR_H #include #include diff --git a/modules/core/include/visp3/core/vpQuaternionVector.h b/modules/core/include/visp3/core/vpQuaternionVector.h index b27d824f80..5f82af0d3b 100644 --- a/modules/core/include/visp3/core/vpQuaternionVector.h +++ b/modules/core/include/visp3/core/vpQuaternionVector.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -38,8 +38,8 @@ * operations on it. */ -#ifndef _vpQuaternionVector_h_ -#define _vpQuaternionVector_h_ +#ifndef VP_QUATERNION_VECTOR_H +#define VP_QUATERNION_VECTOR_H #include #include diff --git a/modules/core/include/visp3/core/vpRGBa.h b/modules/core/include/visp3/core/vpRGBa.h index 9157ff2a26..145ee335d4 100644 --- a/modules/core/include/visp3/core/vpRGBa.h +++ b/modules/core/include/visp3/core/vpRGBa.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -37,8 +37,8 @@ images (it defines a RGB 32 bits structure, fourth byte is not used) */ -#ifndef _vpRGBa_h_ -#define _vpRGBa_h_ +#ifndef VP_RGBA_H +#define VP_RGBA_H #include #include diff --git a/modules/core/include/visp3/core/vpRGBf.h b/modules/core/include/visp3/core/vpRGBf.h index 1bccac9ac4..865b7c6306 100644 --- a/modules/core/include/visp3/core/vpRGBf.h +++ b/modules/core/include/visp3/core/vpRGBf.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -37,8 +37,8 @@ images (it defines a RGB 32-bit floating point structure) */ -#ifndef _vpRGBf_h_ -#define _vpRGBf_h_ +#ifndef VP_RGBF_H +#define VP_RGBF_H #include #include diff --git a/modules/core/include/visp3/core/vpRansac.h b/modules/core/include/visp3/core/vpRansac.h index f795ed7987..20cc8ffc36 100644 --- a/modules/core/include/visp3/core/vpRansac.h +++ b/modules/core/include/visp3/core/vpRansac.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -35,13 +35,12 @@ \file vpRansac.h */ -#ifndef _vpRANSAC_H_ -#define _vpRANSAC_H_ +#ifndef VP_RANSAC_H +#define VP_RANSAC_H #include #include #include -#include // debug and trace #include #include // random number generation @@ -163,11 +162,7 @@ bool vpRansac::ransac(unsigned int npts, const vpColVector &x, if (count > maxDataTrials) { delete[] ind; - vpERROR_TRACE("Unable to select a nondegenerate data set"); throw(vpException(vpException::fatalError, "Unable to select a non degenerate data set")); - /* - // return false; //Useless after a throw() function - */ } } // Fit model to this random selection of data points. @@ -218,7 +213,7 @@ bool vpRansac::ransac(unsigned int npts, const vpColVector &x, trialcount = trialcount + 1; // Safeguard against being stuck in this loop forever if (trialcount > maxTrials) { - vpTRACE("ransac reached the maximum number of %d trials", maxTrials); + std::cout << "Warning: ransac reached the maximum number of " << maxTrials << " trials" << std::endl; break; } } @@ -229,7 +224,7 @@ bool vpRansac::ransac(unsigned int npts, const vpColVector &x, inliers = bestinliers; } else { - vpTRACE("ransac was unable to find a useful solution"); + std::cout << "Warning: ransac was unable to find a useful solution" << std::endl; M = 0; } diff --git a/modules/core/include/visp3/core/vpRect.h b/modules/core/include/visp3/core/vpRect.h index 797c535a6b..40060ed70e 100644 --- a/modules/core/include/visp3/core/vpRect.h +++ b/modules/core/include/visp3/core/vpRect.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,8 +31,8 @@ * Defines a rectangle in the plane. */ -#ifndef _vpRect_h_ -#define _vpRect_h_ +#ifndef VP_RECT_H +#define VP_RECT_H #include #include diff --git a/modules/core/include/visp3/core/vpRectOriented.h b/modules/core/include/visp3/core/vpRectOriented.h index 26397a196e..1fbf7bfb27 100644 --- a/modules/core/include/visp3/core/vpRectOriented.h +++ b/modules/core/include/visp3/core/vpRectOriented.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,8 +31,8 @@ * Defines a (possibly oriented) rectangle in the plane. */ -#ifndef _vpRectOriented_h_ -#define _vpRectOriented_h_ +#ifndef VP_RECT_ORIENTED_H +#define VP_RECT_ORIENTED_H #include #include diff --git a/modules/core/include/visp3/core/vpRobust.h b/modules/core/include/visp3/core/vpRobust.h index 23e3b42b66..70c0ed615e 100644 --- a/modules/core/include/visp3/core/vpRobust.h +++ b/modules/core/include/visp3/core/vpRobust.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -35,8 +35,8 @@ \file vpRobust.h */ -#ifndef vpRobust_h -#define vpRobust_h +#ifndef VP_ROBUST_H +#define VP_ROBUST_H #include #include diff --git a/modules/core/include/visp3/core/vpRotationMatrix.h b/modules/core/include/visp3/core/vpRotationMatrix.h index 9a740665ce..28afed04a6 100644 --- a/modules/core/include/visp3/core/vpRotationMatrix.h +++ b/modules/core/include/visp3/core/vpRotationMatrix.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,8 +36,8 @@ \brief Class that consider the particular case of rotation matrix */ -#ifndef _vpRotationMatrix_h_ -#define _vpRotationMatrix_h_ +#ifndef VP_ROTATION_MATRIX_H +#define VP_ROTATION_MATRIX_H #include #include diff --git a/modules/core/include/visp3/core/vpRotationVector.h b/modules/core/include/visp3/core/vpRotationVector.h index c6bd3c47b6..c2481632c4 100644 --- a/modules/core/include/visp3/core/vpRotationVector.h +++ b/modules/core/include/visp3/core/vpRotationVector.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -37,8 +37,8 @@ (cannot be used as is !) */ -#ifndef _vpRotationVector_h_ -#define _vpRotationVector_h_ +#ifndef VP_ROTATION_VECTOR_H +#define VP_ROTATION_VECTOR_H #include #include diff --git a/modules/core/include/visp3/core/vpRowVector.h b/modules/core/include/visp3/core/vpRowVector.h index 8154ab01e0..88cf80bee3 100644 --- a/modules/core/include/visp3/core/vpRowVector.h +++ b/modules/core/include/visp3/core/vpRowVector.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,8 +31,8 @@ * Operation on row vectors. */ -#ifndef _vpRowVector_H_ -#define _vpRowVector_H_ +#ifndef VP_ROW_VECTOR_H +#define VP_ROW_VECTOR_H #include diff --git a/modules/core/include/visp3/core/vpRxyzVector.h b/modules/core/include/visp3/core/vpRxyzVector.h index 111bf8172a..8f4a572467 100644 --- a/modules/core/include/visp3/core/vpRxyzVector.h +++ b/modules/core/include/visp3/core/vpRxyzVector.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -41,8 +41,8 @@ Rxyz(phi,theta,psi) = Rot(x,phi)Rot(y,theta)Rot(z,psi) */ -#ifndef _vpRxyzVector_h_ -#define _vpRxyzVector_h_ +#ifndef VP_RXYZ_VECTOR_H +#define VP_RXYZ_VECTOR_H #include #include diff --git a/modules/core/include/visp3/core/vpRzyxVector.h b/modules/core/include/visp3/core/vpRzyxVector.h index 87c39fcfa8..40972dc35a 100644 --- a/modules/core/include/visp3/core/vpRzyxVector.h +++ b/modules/core/include/visp3/core/vpRzyxVector.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -41,8 +41,8 @@ Rzyx(phi,theta,psi) = Rot(z,phi)Rot(y,theta)Rot(x,psi) */ -#ifndef _vpRzyxVector_h_ -#define _vpRzyxVector_h_ +#ifndef VP_RZYX_VECTOR_H +#define VP_RZYX_VECTOR_H #include #include diff --git a/modules/core/include/visp3/core/vpRzyzVector.h b/modules/core/include/visp3/core/vpRzyzVector.h index b5f7164614..7f30bd863b 100644 --- a/modules/core/include/visp3/core/vpRzyzVector.h +++ b/modules/core/include/visp3/core/vpRzyzVector.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -40,8 +40,8 @@ Rzyz(phi,theta,psi) = Rot(z,phi)Rot(y,theta)Rot(z,psi) */ -#ifndef _vpRzyzVector_h_ -#define _vpRzyzVector_h_ +#ifndef VP_RZYZ_VECTOR_H +#define VP_RZYZ_VECTOR_H #include #include diff --git a/modules/core/include/visp3/core/vpSphere.h b/modules/core/include/visp3/core/vpSphere.h index ebcdd14427..bb0a763742 100644 --- a/modules/core/include/visp3/core/vpSphere.h +++ b/modules/core/include/visp3/core/vpSphere.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,11 +36,10 @@ * \brief forward projection of a sphere */ -#ifndef _vpSphere_h_ -#define _vpSphere_h_ +#ifndef VP_SPHERE_H +#define VP_SPHERE_H #include -#include #include #include @@ -102,14 +101,14 @@ class VISP_EXPORT vpSphere : public vpForwardProjection double get_x() const { return p[0]; } double get_y() const { return p[1]; } - double get_n20() const { return p[2]; } - double get_n11() const { return p[3]; } - double get_n02() const { return p[4]; } + double get_n20() const { const unsigned int index_2 = 2; return p[index_2]; } + double get_n11() const { const unsigned int index_3 = 3; return p[index_3]; } + double get_n02() const { const unsigned int index_4 = 4; return p[index_4]; } - double getX() const { return cP[0]; } - double getY() const { return cP[1]; } - double getZ() const { return cP[2]; } - double getR() const { return cP[3]; } + double getX() const { const unsigned int index_0 = 0; return cP[index_0]; } + double getY() const { const unsigned int index_1 = 1; return cP[index_1]; } + double getZ() const { const unsigned int index_2 = 2; return cP[index_2]; } + double getR() const { const unsigned int index_3 = 3; return cP[index_3]; } void projection() vp_override; @@ -130,21 +129,21 @@ class VISP_EXPORT vpSphere : public vpForwardProjection * returns second order centered moments of the ellipse normalized * by its area that corresponds to \f$n_20 = mu_20/a\f$. */ - vp_deprecated double get_mu20() const { return p[2]; } + vp_deprecated double get_mu20() const { const unsigned int index_2 = 2; return p[index_2]; } /*! * \deprecated You should rather use get_n11(). * This function is incorrectly named and is confusing since it * returns second order centered moments of the ellipse normalized * by its area that corresponds to \f$n_11 = mu_11/a\f$. */ - vp_deprecated double get_mu11() const { return p[3]; } + vp_deprecated double get_mu11() const { const unsigned int index_3 = 3; return p[index_3]; } /*! * \deprecated You should rather use get_n02(). * This function is incorrectly named and is confusing since it * returns second order centered moments of the ellipse normalized * by its area that corresponds to \f$n_02 = mu_02/a\f$. */ - vp_deprecated double get_mu02() const { return p[4]; } + vp_deprecated double get_mu02() const { const unsigned int index_4 = 4; return p[index_4]; } //@} #endif protected: diff --git a/modules/core/include/visp3/core/vpSubColVector.h b/modules/core/include/visp3/core/vpSubColVector.h index 668aa200e1..949d026a65 100644 --- a/modules/core/include/visp3/core/vpSubColVector.h +++ b/modules/core/include/visp3/core/vpSubColVector.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -37,8 +37,8 @@ * \brief Definition of the vpSubColVector class */ -#ifndef _vpSubColVector_h_ -#define _vpSubColVector_h_ +#ifndef VP_SUBCOL_VECTOR_H +#define VP_SUBCOL_VECTOR_H #include #include diff --git a/modules/core/include/visp3/core/vpSubMatrix.h b/modules/core/include/visp3/core/vpSubMatrix.h index 8177146d1a..73536d5304 100644 --- a/modules/core/include/visp3/core/vpSubMatrix.h +++ b/modules/core/include/visp3/core/vpSubMatrix.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -37,8 +37,8 @@ * \brief Definition of the vpSubMatrix class */ -#ifndef _vpSubMatrix_h_ -#define _vpSubMatrix_h_ +#ifndef VP_SUB_MATRIX_H +#define VP_SUB_MATRIX_H #include #include diff --git a/modules/core/include/visp3/core/vpSubRowVector.h b/modules/core/include/visp3/core/vpSubRowVector.h index f90d5afd5c..b3f3f6f8de 100644 --- a/modules/core/include/visp3/core/vpSubRowVector.h +++ b/modules/core/include/visp3/core/vpSubRowVector.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -37,8 +37,8 @@ * \brief Definition of the vpSubRowVector class */ -#ifndef _vpSubRowVector_h_ -#define _vpSubRowVector_h_ +#ifndef VP_SUB_ROW_VECTOR_H +#define VP_SUB_ROW_VECTOR_H #include #include diff --git a/modules/core/include/visp3/core/vpThetaUVector.h b/modules/core/include/visp3/core/vpThetaUVector.h index 998dca9bf3..8d5ceab495 100644 --- a/modules/core/include/visp3/core/vpThetaUVector.h +++ b/modules/core/include/visp3/core/vpThetaUVector.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -37,8 +37,8 @@ rotation */ -#ifndef _vpThetaUVector_h_ -#define _vpThetaUVector_h_ +#ifndef VP_THETAU_VECTOR_H +#define VP_THETAU_VECTOR_H #include #include diff --git a/modules/core/include/visp3/core/vpTime.h b/modules/core/include/visp3/core/vpTime.h index e43e832264..9233fa3585 100644 --- a/modules/core/include/visp3/core/vpTime.h +++ b/modules/core/include/visp3/core/vpTime.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,8 +36,8 @@ \brief Time management and measurement */ -#ifndef _vpTime_h_ -#define _vpTime_h_ +#ifndef VP_TIME_H +#define VP_TIME_H #include #include diff --git a/modules/core/include/visp3/core/vpTracker.h b/modules/core/include/visp3/core/vpTracker.h index 693f2727b2..0ad1c44372 100644 --- a/modules/core/include/visp3/core/vpTracker.h +++ b/modules/core/include/visp3/core/vpTracker.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,8 +36,8 @@ * \brief Class that defines what is a generic tracker. */ -#ifndef _vpTracker_H_ -#define _vpTracker_H_ +#ifndef VP_TRACKER_H +#define VP_TRACKER_H #include #include diff --git a/modules/core/include/visp3/core/vpTrackingException.h b/modules/core/include/visp3/core/vpTrackingException.h index 0c4f37cda9..dd29883f17 100644 --- a/modules/core/include/visp3/core/vpTrackingException.h +++ b/modules/core/include/visp3/core/vpTrackingException.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,8 +36,8 @@ * \brief error that can be emitted by the vpTracker class and its derivatives */ -#ifndef _vpTrackingException_H_ -#define _vpTrackingException_H_ +#ifndef VP_TRACKING_EXCEPTION_H +#define VP_TRACKING_EXCEPTION_H #include #include diff --git a/modules/core/include/visp3/core/vpTranslationVector.h b/modules/core/include/visp3/core/vpTranslationVector.h index 48ee1c194f..1317e2dc44 100644 --- a/modules/core/include/visp3/core/vpTranslationVector.h +++ b/modules/core/include/visp3/core/vpTranslationVector.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,8 +36,8 @@ * \brief Class that consider the case of a translation vector. */ -#ifndef _vpTRANSLATIONVECTOR_H_ -#define _vpTRANSLATIONVECTOR_H_ +#ifndef VP_TRANSLATION_VECTOR_H +#define VP_TRANSLATION_VECTOR_H #include #include @@ -72,7 +72,7 @@ class vpMatrix; \endcode You can also initialize the vector using operator<<(double): \code - t << 0, 0.1, 05; + t << 0, 0.1, 0.5; \endcode Or you can also initialize the vector from a list of doubles if ViSP is build with c++11 enabled: \code diff --git a/modules/core/include/visp3/core/vpUniRand.h b/modules/core/include/visp3/core/vpUniRand.h index df7030aa98..4c9b0fde0b 100644 --- a/modules/core/include/visp3/core/vpUniRand.h +++ b/modules/core/include/visp3/core/vpUniRand.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -61,8 +61,8 @@ * your project. */ -#ifndef _vpUniRand_h_ -#define _vpUniRand_h_ +#ifndef VP_UNIRAND_H +#define VP_UNIRAND_H #include // Visual Studio 2010 or previous is missing inttypes.h @@ -155,17 +155,17 @@ class VISP_EXPORT vpUniRand } private: - struct vpPcgStateSetseq_64_t + struct vpPcgStateSetSeq64t { // Internals are *Private*. uint64_t state; // RNG state. All values are possible. uint64_t inc; // Controls which RNG sequence (stream) is // selected. Must *always* be odd. - vpPcgStateSetseq_64_t(uint64_t state_ = 0x853c49e6748fea9bULL, uint64_t inc_ = 0xda3e39cb94b95bdbULL) + vpPcgStateSetSeq64t(uint64_t state_ = 0x853c49e6748fea9bULL, uint64_t inc_ = 0xda3e39cb94b95bdbULL) : state(state_), inc(inc_) { } }; - typedef struct vpPcgStateSetseq_64_t pcg32_random_t; + typedef struct vpPcgStateSetSeq64t pcg32_random_t; private: uint32_t boundedRand(uint32_t bound); diff --git a/modules/core/include/visp3/core/vpVelocityTwistMatrix.h b/modules/core/include/visp3/core/vpVelocityTwistMatrix.h index d7044aeb4a..2f461e59af 100644 --- a/modules/core/include/visp3/core/vpVelocityTwistMatrix.h +++ b/modules/core/include/visp3/core/vpVelocityTwistMatrix.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,8 +31,8 @@ * Velocity twist transformation matrix. */ -#ifndef _vpVelocityTwistMatrix_h_ -#define _vpVelocityTwistMatrix_h_ +#ifndef VP_VELOCITY_TWIST_MATRIX_H +#define VP_VELOCITY_TWIST_MATRIX_H #include #include diff --git a/modules/core/include/visp3/core/vpXmlParserCamera.h b/modules/core/include/visp3/core/vpXmlParserCamera.h index c36ae99d5b..5dcca2a084 100644 --- a/modules/core/include/visp3/core/vpXmlParserCamera.h +++ b/modules/core/include/visp3/core/vpXmlParserCamera.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -38,8 +38,8 @@ */ -#ifndef _vpXMLPARSERCAMERA_H_ -#define _vpXMLPARSERCAMERA_H_ +#ifndef VP_XML_PARSER_CAMERA_H +#define VP_XML_PARSER_CAMERA_H #include @@ -193,7 +193,7 @@ class VISP_EXPORT vpXmlParserCamera private: vpXmlParserCamera(const vpXmlParserCamera &c); // noncopyable - vpXmlParserCamera &operator=(const vpXmlParserCamera &); // + vpXmlParserCamera &operator=(const vpXmlParserCamera &c); // // PIMPL idiom class Impl; diff --git a/modules/core/include/visp3/core/vpXmlParserHomogeneousMatrix.h b/modules/core/include/visp3/core/vpXmlParserHomogeneousMatrix.h index 62fae4ac1e..7c52296d5a 100644 --- a/modules/core/include/visp3/core/vpXmlParserHomogeneousMatrix.h +++ b/modules/core/include/visp3/core/vpXmlParserHomogeneousMatrix.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -39,8 +39,8 @@ */ -#ifndef _VP_XMLPARSERHOMOGENEOUSMATRIX_H_ -#define _VP_XMLPARSERHOMOGENEOUSMATRIX_H_ +#ifndef VP_XML_PARSER_HOMOGENEOUS_MATRIX_H +#define VP_XML_PARSER_HOMOGENEOUS_MATRIX_H #include diff --git a/modules/core/include/visp3/core/vpXmlParserRectOriented.h b/modules/core/include/visp3/core/vpXmlParserRectOriented.h index adb5a6d191..2ab2f329f4 100644 --- a/modules/core/include/visp3/core/vpXmlParserRectOriented.h +++ b/modules/core/include/visp3/core/vpXmlParserRectOriented.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -37,8 +37,8 @@ Class vpXmlParserRectOriented allows to load and save oriented rectangles in a file XML */ -#ifndef _vpXmlParserRectOriented_h_ -#define _vpXmlParserRectOriented_h_ +#ifndef VP_XML_PARSER_RECT_ORIENTED_H +#define VP_XML_PARSER_RECT_ORIENTED_H #include diff --git a/modules/core/src/camera/vpCameraParameters.cpp b/modules/core/src/camera/vpCameraParameters.cpp index db79745b47..d3ab550f0b 100644 --- a/modules/core/src/camera/vpCameraParameters.cpp +++ b/modules/core/src/camera/vpCameraParameters.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -43,7 +43,6 @@ #include #include #include -#include #include #include #include @@ -347,13 +346,17 @@ void vpCameraParameters::init(const vpCameraParameters &c) { *this = c; } */ void vpCameraParameters::initFromCalibrationMatrix(const vpMatrix &K) { - if ((K.getRows() != 3) || (K.getCols() != 3)) { + const unsigned int nparam = 3; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + if ((K.getRows() != nparam) || (K.getCols() != nparam)) { throw vpException(vpException::dimensionError, "bad size for calibration matrix"); } - if (std::fabs(K[2][2] - 1.0) > std::numeric_limits::epsilon()) { + if (std::fabs(K[index_2][index_2] - 1.0) > std::numeric_limits::epsilon()) { throw vpException(vpException::badValue, "bad value: K[2][2] must be equal to 1"); } - initPersProjWithoutDistortion(K[0][0], K[1][1], K[0][2], K[1][2]); + initPersProjWithoutDistortion(K[index_0][index_0], K[index_1][index_1], K[index_0][index_2], K[index_1][index_2]); } /*! @@ -397,8 +400,8 @@ void vpCameraParameters::initFromFov(const unsigned int &w, const unsigned int & m_projModel = vpCameraParameters::perspectiveProjWithoutDistortion; m_u0 = static_cast(w) / 2.; m_v0 = static_cast(h) / 2.; - m_px = m_u0 / tan(hfov / 2); - m_py = m_v0 / tan(vfov / 2); + m_px = m_u0 / tan(hfov / 2.); + m_py = m_v0 / tan(vfov / 2.); m_kud = 0; m_kdu = 0; m_inv_px = 1. / m_px; @@ -507,7 +510,13 @@ void vpCameraParameters::computeFov(const unsigned int &w, const unsigned int &h { bool cond1 = (!m_isFov) || (w != m_width) || (h != m_height); if (cond1 && (w != 0) && (h != 0)) { - m_fovNormals = std::vector(4); + const unsigned int nparam_3 = 3; + const unsigned int nparam_4 = 4; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + m_fovNormals = std::vector(nparam_4); m_isFov = true; @@ -519,7 +528,7 @@ void vpCameraParameters::computeFov(const unsigned int &w, const unsigned int &h m_width = w; m_height = h; - vpColVector n(3); + vpColVector n(nparam_3); n = 0; n[0] = 1.0; @@ -529,10 +538,10 @@ void vpCameraParameters::computeFov(const unsigned int &w, const unsigned int &h vpColVector nLeft, nRight; nLeft = Rleft * (-n); - m_fovNormals[0] = nLeft.normalize(); + m_fovNormals[index_0] = nLeft.normalize(); nRight = Rright * n; - m_fovNormals[1] = nRight.normalize(); + m_fovNormals[index_1] = nRight.normalize(); n = 0; n[1] = 1.0; @@ -543,10 +552,10 @@ void vpCameraParameters::computeFov(const unsigned int &w, const unsigned int &h vpColVector nUp, nDown; nUp = Rup * (-n); - m_fovNormals[2] = nUp.normalize(); + m_fovNormals[index_2] = nUp.normalize(); nDown = Rdown * n; - m_fovNormals[3] = nDown.normalize(); + m_fovNormals[index_3] = nDown.normalize(); m_hFovAngle = hFovAngle + minushFovAngle; m_vFovAngle = vFovAngle + minusvFovAngle; @@ -566,12 +575,15 @@ void vpCameraParameters::computeFov(const unsigned int &w, const unsigned int &h */ vpMatrix vpCameraParameters::get_K() const { + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; vpMatrix K(3, 3, 0.); - K[0][0] = m_px; - K[1][1] = m_py; - K[0][2] = m_u0; - K[1][2] = m_v0; - K[2][2] = 1.0; + K[index_0][index_0] = m_px; + K[index_1][index_1] = m_py; + K[index_0][index_2] = m_u0; + K[index_1][index_2] = m_v0; + K[index_2][index_2] = 1.0; return K; } @@ -588,12 +600,16 @@ vpMatrix vpCameraParameters::get_K() const */ vpMatrix vpCameraParameters::get_K_inverse() const { + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + vpMatrix K_inv(3, 3, 0.); - K_inv[0][0] = m_inv_px; - K_inv[1][1] = m_inv_py; - K_inv[0][2] = -m_u0 * m_inv_px; - K_inv[1][2] = -m_v0 * m_inv_py; - K_inv[2][2] = 1.0; + K_inv[index_0][index_0] = m_inv_px; + K_inv[index_1][index_1] = m_inv_py; + K_inv[index_0][index_2] = -m_u0 * m_inv_px; + K_inv[index_1][index_2] = -m_v0 * m_inv_py; + K_inv[index_2][index_2] = 1.0; return K_inv; } @@ -657,7 +673,8 @@ VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpCameraParameters } case vpCameraParameters::perspectiveProjWithDistortion: { std::ios_base::fmtflags original_flags = os.flags(); - os.precision(10); + const unsigned int precision = 10; + os.precision(precision); os << "Camera parameters for perspective projection with distortion:" << std::endl; os << " px = " << cam.get_px() << "\t py = " << cam.get_py() << std::endl; os << " u0 = " << cam.get_u0() << "\t v0 = " << cam.get_v0() << std::endl; @@ -668,8 +685,7 @@ VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpCameraParameters } case vpCameraParameters::ProjWithKannalaBrandtDistortion: { os << "Camera parameters for projection with Kannala-Brandt distortion:" << std::endl; - os << " px = " << cam.get_px() << "\t py = " << cam.get_py() << std::endl; - os << " u0 = " << cam.get_u0() << "\t v0 = " << cam.get_v0() << std::endl; + os << " px = " << cam.get_px() << "\t py = " << cam.get_py() << std::endl << " u0 = " << cam.get_u0() << "\t v0 = " << cam.get_v0() << std::endl; os << " Coefficients: "; std::vector tmp_coefs = cam.getKannalaBrandtDistortionCoefficients(); unsigned int tmp_coefs_size = tmp_coefs.size(); diff --git a/modules/core/src/camera/vpColorDepthConversion.cpp b/modules/core/src/camera/vpColorDepthConversion.cpp index b57edb5ef0..df2adc8920 100644 --- a/modules/core/src/camera/vpColorDepthConversion.cpp +++ b/modules/core/src/camera/vpColorDepthConversion.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -200,19 +200,21 @@ vpImagePoint vpColorDepthConversion::projectColorToDepth( for (auto curr_pixel = start_pixel; curr_pixel.inSegment(start_pixel, end_pixel) && (curr_pixel != end_pixel); curr_pixel = curr_pixel.nextInSegment(start_pixel, end_pixel)) { const auto depth = depth_scale * data[static_cast((curr_pixel.get_v() * depth_width) + curr_pixel.get_u())]; + bool stop_for_loop = false; if (std::fabs(depth) <= std::numeric_limits::epsilon()) { - continue; + stop_for_loop = true; } + if (!stop_for_loop) { + const auto point = deproject(depth_intrinsics, curr_pixel, depth); + const auto transformed_point = transform(color_M_depth, point); + const auto projected_pixel = project(color_intrinsics, transformed_point); - const auto point = deproject(depth_intrinsics, curr_pixel, depth); - const auto transformed_point = transform(color_M_depth, point); - const auto projected_pixel = project(color_intrinsics, transformed_point); - - const auto new_dist = vpMath::sqr(projected_pixel.get_v() - from_pixel.get_v()) + - vpMath::sqr(projected_pixel.get_u() - from_pixel.get_u()); - if ((new_dist < min_dist) || (min_dist < 0)) { - min_dist = new_dist; - depth_pixel = curr_pixel; + const auto new_dist = vpMath::sqr(projected_pixel.get_v() - from_pixel.get_v()) + + vpMath::sqr(projected_pixel.get_u() - from_pixel.get_u()); + if ((new_dist < min_dist) || (min_dist < 0)) { + min_dist = new_dist; + depth_pixel = curr_pixel; + } } } @@ -236,18 +238,22 @@ vpImagePoint vpColorDepthConversion::projectColorToDepth( for (vpImagePoint curr_pixel = start_pixel; curr_pixel.inSegment(start_pixel, end_pixel) && curr_pixel != end_pixel; curr_pixel = curr_pixel.nextInSegment(start_pixel, end_pixel)) { const double depth = depth_scale * data[static_cast(curr_pixel.get_v() * depth_width + curr_pixel.get_u())]; - if (std::fabs(depth) <= std::numeric_limits::epsilon()) - continue; - const vpColVector point = deproject(depth_intrinsics, curr_pixel, depth); - const vpColVector transformed_point = transform(color_M_depth, point); - const vpImagePoint projected_pixel = project(color_intrinsics, transformed_point); + bool stop_for_loop = false; + if (std::fabs(depth) <= std::numeric_limits::epsilon()) { + stop_for_loop = true; + } + if (!stop_for_loop) { + const vpColVector point = deproject(depth_intrinsics, curr_pixel, depth); + const vpColVector transformed_point = transform(color_M_depth, point); + const vpImagePoint projected_pixel = project(color_intrinsics, transformed_point); - const double new_dist = vpMath::sqr(projected_pixel.get_v() - from_pixel.get_v()) + - vpMath::sqr(projected_pixel.get_u() - from_pixel.get_u()); - if (new_dist < min_dist || min_dist < 0) { - min_dist = new_dist; - depth_pixel = curr_pixel; + const double new_dist = vpMath::sqr(projected_pixel.get_v() - from_pixel.get_v()) + + vpMath::sqr(projected_pixel.get_u() - from_pixel.get_u()); + if (new_dist < min_dist || min_dist < 0) { + min_dist = new_dist; + depth_pixel = curr_pixel; + } } } #endif diff --git a/modules/core/src/camera/vpMeterPixelConversion.cpp b/modules/core/src/camera/vpMeterPixelConversion.cpp index 38aaf8f2d4..520daf3586 100644 --- a/modules/core/src/camera/vpMeterPixelConversion.cpp +++ b/modules/core/src/camera/vpMeterPixelConversion.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -39,7 +38,6 @@ */ #include -#include #include #include #include @@ -62,7 +60,6 @@ void vpMeterPixelConversion::convertLine(const vpCameraParameters &cam, const do double d = sqrt(vpMath::sqr(cam.m_py * co) + vpMath::sqr(cam.m_px * si)); if (fabs(d) < 1e-6) { - vpERROR_TRACE("division by zero"); throw(vpException(vpException::divideByZeroError, "division by zero")); } @@ -101,11 +98,16 @@ void vpMeterPixelConversion::convertEllipse(const vpCameraParameters &cam, const vpImagePoint ¢er_p, double &n20_p, double &n11_p, double &n02_p) { // Get the parameters of the ellipse in the image plane - double xc_m = circle.p[0]; - double yc_m = circle.p[1]; - double n20_m = circle.p[2]; - double n11_m = circle.p[3]; - double n02_m = circle.p[4]; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + double xc_m = circle.p[index_0]; + double yc_m = circle.p[index_1]; + double n20_m = circle.p[index_2]; + double n11_m = circle.p[index_3]; + double n02_m = circle.p[index_4]; // Convert from meter to pixels vpMeterPixelConversion::convertPoint(cam, xc_m, yc_m, center_p); @@ -144,11 +146,16 @@ void vpMeterPixelConversion::convertEllipse(const vpCameraParameters &cam, const vpImagePoint ¢er_p, double &n20_p, double &n11_p, double &n02_p) { // Get the parameters of the ellipse in the image plane - double xc_m = sphere.p[0]; - double yc_m = sphere.p[1]; - double n20_m = sphere.p[2]; - double n11_m = sphere.p[3]; - double n02_m = sphere.p[4]; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + double xc_m = sphere.p[index_0]; + double yc_m = sphere.p[index_1]; + double n20_m = sphere.p[index_2]; + double n11_m = sphere.p[index_3]; + double n02_m = sphere.p[index_4]; // Convert from meter to pixels vpMeterPixelConversion::convertPoint(cam, xc_m, yc_m, center_p); @@ -210,7 +217,6 @@ void vpMeterPixelConversion::convertLine(const cv::Mat &cameraMatrix, const doub double d = sqrt(vpMath::sqr(py * co) + vpMath::sqr(px * si)); if (fabs(d) < 1e-6) { - vpERROR_TRACE("division by zero"); throw(vpException(vpException::divideByZeroError, "division by zero")); } @@ -251,15 +257,21 @@ void vpMeterPixelConversion::convertLine(const cv::Mat &cameraMatrix, const doub void vpMeterPixelConversion::convertEllipse(const cv::Mat &cameraMatrix, const vpCircle &circle, vpImagePoint ¢er, double &n20_p, double &n11_p, double &n02_p) { - double px = cameraMatrix.at(0, 0); - double py = cameraMatrix.at(1, 1); - cv::Mat distCoeffs = cv::Mat::zeros(5, 1, CV_64FC1); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + const unsigned int index_5 = 5; + double px = cameraMatrix.at(index_0, index_0); + double py = cameraMatrix.at(index_1, index_1); + cv::Mat distCoeffs = cv::Mat::zeros(index_5, index_1, CV_64FC1); // Get the parameters of the ellipse in the image plane - double xc_m = circle.p[0]; - double yc_m = circle.p[1]; - double n20_m = circle.p[2]; - double n11_m = circle.p[3]; - double n02_m = circle.p[4]; + double xc_m = circle.p[index_0]; + double yc_m = circle.p[index_1]; + double n20_m = circle.p[index_2]; + double n11_m = circle.p[index_3]; + double n02_m = circle.p[index_4]; // Convert from meter to pixels vpMeterPixelConversion::convertPoint(cameraMatrix, distCoeffs, xc_m, yc_m, center); @@ -300,15 +312,21 @@ void vpMeterPixelConversion::convertEllipse(const cv::Mat &cameraMatrix, const v void vpMeterPixelConversion::convertEllipse(const cv::Mat &cameraMatrix, const vpSphere &sphere, vpImagePoint ¢er, double &n20_p, double &n11_p, double &n02_p) { - double px = cameraMatrix.at(0, 0); - double py = cameraMatrix.at(1, 1); - cv::Mat distCoeffs = cv::Mat::zeros(5, 1, CV_64FC1); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + const unsigned int index_5 = 5; + double px = cameraMatrix.at(index_0, index_0); + double py = cameraMatrix.at(index_1, index_1); + cv::Mat distCoeffs = cv::Mat::zeros(index_5, index_1, CV_64FC1); // Get the parameters of the ellipse in the image plane - double xc_m = sphere.p[0]; - double yc_m = sphere.p[1]; - double n20_m = sphere.p[2]; - double n11_m = sphere.p[3]; - double n02_m = sphere.p[4]; + double xc_m = sphere.p[index_0]; + double yc_m = sphere.p[index_1]; + double n20_m = sphere.p[index_2]; + double n11_m = sphere.p[index_3]; + double n02_m = sphere.p[index_4]; // Convert from meter to pixels vpMeterPixelConversion::convertPoint(cameraMatrix, distCoeffs, xc_m, yc_m, center); diff --git a/modules/core/src/camera/vpPixelMeterConversion.cpp b/modules/core/src/camera/vpPixelMeterConversion.cpp index 4e0474e05a..62e254f2cc 100644 --- a/modules/core/src/camera/vpPixelMeterConversion.cpp +++ b/modules/core/src/camera/vpPixelMeterConversion.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -38,7 +37,6 @@ \brief Pixel to meter conversion. */ #include -#include #include #include #include @@ -54,9 +52,9 @@ BEGIN_VISP_NAMESPACE * \param[out] xc_m, yc_m : Center of the ellipse with coordinates in meters in the image plane. * \param[out] n20_m, n11_m, n02_m : Normalized second order moments of the ellipse in meters in the image plane. */ -void vpPixelMeterConversion::convertEllipse(const vpCameraParameters &cam, const vpImagePoint ¢er_p, double n20_p, - double n11_p, double n02_p, double &xc_m, double &yc_m, double &n20_m, - double &n11_m, double &n02_m) + void vpPixelMeterConversion::convertEllipse(const vpCameraParameters &cam, const vpImagePoint ¢er_p, double n20_p, + double n11_p, double n02_p, double &xc_m, double &yc_m, double &n20_m, + double &n11_m, double &n02_m) { vpPixelMeterConversion::convertPoint(cam, center_p, xc_m, yc_m); double px = cam.get_px(); @@ -84,7 +82,6 @@ void vpPixelMeterConversion::convertLine(const vpCameraParameters &cam, const do double d = vpMath::sqr(cam.m_px * co) + vpMath::sqr(cam.m_py * si); if (fabs(d) < 1e-6) { - vpERROR_TRACE("division by zero"); throw(vpException(vpException::divideByZeroError, "division by zero")); } theta_m = atan2(si * cam.m_py, co * cam.m_px); @@ -221,7 +218,6 @@ void vpPixelMeterConversion::convertLine(const cv::Mat &cameraMatrix, const doub double d = vpMath::sqr(px * co) + vpMath::sqr(py * si); if (fabs(d) < 1e-6) { - vpERROR_TRACE("division by zero"); throw(vpException(vpException::divideByZeroError, "division by zero")); } theta_m = atan2(si * py, co * px); diff --git a/modules/core/src/camera/vpXmlParserCamera.cpp b/modules/core/src/camera/vpXmlParserCamera.cpp index f36f126c97..6c35a05650 100644 --- a/modules/core/src/camera/vpXmlParserCamera.cpp +++ b/modules/core/src/camera/vpXmlParserCamera.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -44,7 +43,6 @@ #if defined(VISP_HAVE_PUGIXML) #include -#include /* -------------------------------------------------------------------------- */ /* --- LABEL XML ------------------------------------------------------------ @@ -85,8 +83,7 @@ BEGIN_VISP_NAMESPACE #ifndef DOXYGEN_SHOULD_SKIP_THIS class vpXmlParserCamera::Impl { -private: - /* --- XML Code------------------------------------------------------------ +public: /* --- XML Code------------------------------------------------------------ */ enum vpXmlCodeType { @@ -116,7 +113,6 @@ class vpXmlParserCamera::Impl CODE_XML_ADDITIONAL_INFO }; -public: Impl() : camera(), camera_name(), image_width(0), image_height(0), subsampling_width(0), subsampling_height(0), full_width(0), full_height(0) @@ -169,28 +165,29 @@ class vpXmlParserCamera::Impl unsigned int nbCamera = 0; for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) { - if (node.type() != pugi::node_element) - continue; - - if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) { - prop = CODE_XML_OTHER; - back = SEQUENCE_ERROR; - } - if (prop == CODE_XML_CAMERA) { - if (SEQUENCE_OK == read_camera(node, cam_name, projModel, im_width, im_height, subsampl_width, subsampl_height, verbose)) - nbCamera++; + if (node.type() == pugi::node_element) { + if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) { + prop = CODE_XML_OTHER; + back = SEQUENCE_ERROR; + } + if (prop == CODE_XML_CAMERA) { + if (SEQUENCE_OK == read_camera(node, cam_name, projModel, im_width, im_height, subsampl_width, subsampl_height, verbose)) { + ++nbCamera; + } + } + else { + back = SEQUENCE_ERROR; + } } - else - back = SEQUENCE_ERROR; } if (nbCamera == 0) { back = SEQUENCE_ERROR; - vpCERROR << "No camera parameters is available" << std::endl << "with your specifications" << std::endl; + std::cout << "Warning: No camera parameters is available" << std::endl << "with your specifications" << std::endl; } else if (nbCamera > 1) { back = SEQUENCE_ERROR; - vpCERROR << nbCamera << " sets of camera parameters are available" << std::endl + std::cout << "Warning: " << nbCamera << " sets of camera parameters are available" << std::endl << "with your specifications : " << std::endl << "precise your choice..." << std::endl; } @@ -232,68 +229,67 @@ class vpXmlParserCamera::Impl vpXmlCodeSequenceType back = SEQUENCE_OK; for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) { - if (node.type() != pugi::node_element) - continue; - - if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) { - prop = CODE_XML_OTHER; - back = SEQUENCE_ERROR; - } + if (node.type() == pugi::node_element) { + if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) { + prop = CODE_XML_OTHER; + back = SEQUENCE_ERROR; + } - switch (prop) { - case CODE_XML_CAMERA_NAME: { - camera_name_tmp = node.text().as_string(); - if (verbose) { - std::cout << "Found camera with name: \"" << camera_name_tmp << "\"" << std::endl; + switch (prop) { + case CODE_XML_CAMERA_NAME: { + camera_name_tmp = node.text().as_string(); + if (verbose) { + std::cout << "Found camera with name: \"" << camera_name_tmp << "\"" << std::endl; + } + break; } - break; - } - case CODE_XML_WIDTH: - image_width_tmp = node.text().as_uint(); - break; - - case CODE_XML_HEIGHT: - image_height_tmp = node.text().as_uint(); - break; - case CODE_XML_SUBSAMPLING_WIDTH: - subsampling_width_tmp = node.text().as_uint(); - break; - case CODE_XML_SUBSAMPLING_HEIGHT: - subsampling_height_tmp = node.text().as_uint(); - break; - - case CODE_XML_MODEL: - back = read_camera_model(node, cam_tmp_model); - if (cam_tmp_model.get_projModel() == projModel) { - cam_tmp = cam_tmp_model; - same_proj_model = true; // Same projection model + case CODE_XML_WIDTH: + image_width_tmp = node.text().as_uint(); + break; + + case CODE_XML_HEIGHT: + image_height_tmp = node.text().as_uint(); + break; + case CODE_XML_SUBSAMPLING_WIDTH: + subsampling_width_tmp = node.text().as_uint(); + break; + case CODE_XML_SUBSAMPLING_HEIGHT: + subsampling_height_tmp = node.text().as_uint(); + break; + + case CODE_XML_MODEL: + back = read_camera_model(node, cam_tmp_model); + if (cam_tmp_model.get_projModel() == projModel) { + cam_tmp = cam_tmp_model; + same_proj_model = true; // Same projection model + } + break; + + case CODE_XML_ADDITIONAL_INFO: + break; + + case CODE_XML_BAD: + case CODE_XML_OTHER: + case CODE_XML_CAMERA: + case CODE_XML_FULL_HEIGHT: + case CODE_XML_FULL_WIDTH: + case CODE_XML_MODEL_TYPE: + case CODE_XML_U0: + case CODE_XML_V0: + case CODE_XML_PX: + case CODE_XML_PY: + case CODE_XML_KUD: + case CODE_XML_KDU: + case CODE_XML_K1: + case CODE_XML_K2: + case CODE_XML_K3: + case CODE_XML_K4: + case CODE_XML_K5: + default: + back = SEQUENCE_ERROR; + + break; } - break; - - case CODE_XML_ADDITIONAL_INFO: - break; - - case CODE_XML_BAD: - case CODE_XML_OTHER: - case CODE_XML_CAMERA: - case CODE_XML_FULL_HEIGHT: - case CODE_XML_FULL_WIDTH: - case CODE_XML_MODEL_TYPE: - case CODE_XML_U0: - case CODE_XML_V0: - case CODE_XML_PX: - case CODE_XML_PY: - case CODE_XML_KUD: - case CODE_XML_KDU: - case CODE_XML_K1: - case CODE_XML_K2: - case CODE_XML_K3: - case CODE_XML_K4: - case CODE_XML_K5: - default: - back = SEQUENCE_ERROR; - - break; } } // Create a specific test for subsampling_width and subsampling_height to @@ -383,103 +379,101 @@ class vpXmlParserCamera::Impl unsigned int validation = 0; for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) { - // vpDEBUG_TRACE (15, "Carac : %s.", node ->name); - if (node.type() != pugi::node_element) - continue; - - if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) { - prop = CODE_XML_OTHER; - back = SEQUENCE_ERROR; - } + if (node.type() == pugi::node_element) { + if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) { + prop = CODE_XML_OTHER; + back = SEQUENCE_ERROR; + } - switch (prop) { - case CODE_XML_MODEL_TYPE: { - model_type = node.text().as_string(); - nb++; - validation = validation | 0x01; - } break; - case CODE_XML_U0: - u0 = node.text().as_double(); - nb++; - validation = validation | 0x02; - break; - case CODE_XML_V0: - v0 = node.text().as_double(); - nb++; - validation = validation | 0x04; - break; - case CODE_XML_PX: - px = node.text().as_double(); - nb++; - validation = validation | 0x08; - break; - case CODE_XML_PY: - py = node.text().as_double(); - nb++; - validation = validation | 0x10; - break; - case CODE_XML_KUD: - kud = node.text().as_double(); - nb++; - validation = validation | 0x20; - break; - case CODE_XML_KDU: - kdu = node.text().as_double(); - nb++; - validation = validation | 0x40; - break; - case CODE_XML_K1: - distortion_coeffs.push_back(node.text().as_double()); - nb++; - validation = validation | 0x20; - break; - case CODE_XML_K2: - distortion_coeffs.push_back(node.text().as_double()); - nb++; - validation = validation | 0x40; - break; - case CODE_XML_K3: - distortion_coeffs.push_back(node.text().as_double()); - nb++; - validation = validation | 0x80; - break; - case CODE_XML_K4: - distortion_coeffs.push_back(node.text().as_double()); - nb++; - validation = validation | 0x100; - break; - case CODE_XML_K5: - distortion_coeffs.push_back(node.text().as_double()); - nb++; - validation = validation | 0x200; - break; - case CODE_XML_BAD: - case CODE_XML_OTHER: - case CODE_XML_CAMERA: - case CODE_XML_CAMERA_NAME: - case CODE_XML_HEIGHT: - case CODE_XML_WIDTH: - case CODE_XML_SUBSAMPLING_WIDTH: - case CODE_XML_SUBSAMPLING_HEIGHT: - case CODE_XML_FULL_HEIGHT: - case CODE_XML_FULL_WIDTH: - case CODE_XML_MODEL: - case CODE_XML_ADDITIONAL_INFO: - default: - back = SEQUENCE_ERROR; - break; + switch (prop) { + case CODE_XML_MODEL_TYPE: { + model_type = node.text().as_string(); + ++nb; + validation = validation | 0x01; + } break; + case CODE_XML_U0: + u0 = node.text().as_double(); + ++nb; + validation = validation | 0x02; + break; + case CODE_XML_V0: + v0 = node.text().as_double(); + ++nb; + validation = validation | 0x04; + break; + case CODE_XML_PX: + px = node.text().as_double(); + ++nb; + validation = validation | 0x08; + break; + case CODE_XML_PY: + py = node.text().as_double(); + ++nb; + validation = validation | 0x10; + break; + case CODE_XML_KUD: + kud = node.text().as_double(); + ++nb; + validation = validation | 0x20; + break; + case CODE_XML_KDU: + kdu = node.text().as_double(); + ++nb; + validation = validation | 0x40; + break; + case CODE_XML_K1: + distortion_coeffs.push_back(node.text().as_double()); + ++nb; + validation = validation | 0x20; + break; + case CODE_XML_K2: + distortion_coeffs.push_back(node.text().as_double()); + ++nb; + validation = validation | 0x40; + break; + case CODE_XML_K3: + distortion_coeffs.push_back(node.text().as_double()); + ++nb; + validation = validation | 0x80; + break; + case CODE_XML_K4: + distortion_coeffs.push_back(node.text().as_double()); + ++nb; + validation = validation | 0x100; + break; + case CODE_XML_K5: + distortion_coeffs.push_back(node.text().as_double()); + ++nb; + validation = validation | 0x200; + break; + case CODE_XML_BAD: + case CODE_XML_OTHER: + case CODE_XML_CAMERA: + case CODE_XML_CAMERA_NAME: + case CODE_XML_HEIGHT: + case CODE_XML_WIDTH: + case CODE_XML_SUBSAMPLING_WIDTH: + case CODE_XML_SUBSAMPLING_HEIGHT: + case CODE_XML_FULL_HEIGHT: + case CODE_XML_FULL_WIDTH: + case CODE_XML_MODEL: + case CODE_XML_ADDITIONAL_INFO: + default: + back = SEQUENCE_ERROR; + break; + } } } if (model_type.empty()) { - vpERROR_TRACE("projection model type doesn't match with any known model !"); + std::cout << "Warning: projection model type doesn't match with any known model !" << std::endl; return SEQUENCE_ERROR; } if (!strcmp(model_type.c_str(), LABEL_XML_MODEL_WITHOUT_DISTORTION)) { if (nb != 5 || validation != 0x001F) { - vpCERROR << "ERROR in 'model' field:\n"; - vpCERROR << "it must contain 5 parameters\n"; + std::cout << "ERROR in 'model' field:\n"; + std::cout << "it must contain 5 parameters\n"; return SEQUENCE_ERROR; } @@ -487,8 +481,8 @@ class vpXmlParserCamera::Impl } else if (!strcmp(model_type.c_str(), LABEL_XML_MODEL_WITH_DISTORTION)) { if (nb != 7 || validation != 0x7F) { - vpCERROR << "ERROR in 'model' field:\n"; - vpCERROR << "it must contain 7 parameters\n"; + std::cout << "ERROR in 'model' field:\n"; + std::cout << "it must contain 7 parameters\n"; return SEQUENCE_ERROR; } @@ -496,8 +490,8 @@ class vpXmlParserCamera::Impl } else if (!strcmp(model_type.c_str(), LABEL_XML_MODEL_WITH_KANNALA_BRANDT_DISTORTION)) { if (nb != 10 || validation != 0x3FF) { // at least one coefficient is missing. We should know which one - vpCERROR << "ERROR in 'model' field:\n"; - vpCERROR << "it must contain 10 parameters\n"; + std::cout << "ERROR in 'model' field:\n"; + std::cout << "it must contain 10 parameters\n"; std::vector fixed_distortion_coeffs; @@ -505,16 +499,22 @@ class vpXmlParserCamera::Impl // Since 0x3FF is 0011|1111|1111 and we are interested in the most significant 1s shown below // -- --- // If we divide by 32 (>> 2^5 : 5 remaining least significant bits), we will have to check 5 bits only - int check = validation / 32; + const int dividerForBitCheck = 32; + int check = validation / dividerForBitCheck; int j = 0; - for (int i = 0; i < 5; ++i) { - int bit = check % 2; // if bit == 1 => the corresponding distortion coefficient is present. - if (!bit) + const int nbRemainingBits = 5; + const int moduloForOddity = 2; + const int dividerForRightShift = 2; + for (int i = 0; i < nbRemainingBits; ++i) { + int bit = check % moduloForOddity; // if bit == 1 => the corresponding distortion coefficient is present. + if (!bit) { fixed_distortion_coeffs.push_back(0.); - else + } + else { fixed_distortion_coeffs.push_back(distortion_coeffs[j++]); - check /= 2; + } + check /= dividerForRightShift; } cam_tmp.initProjWithKannalaBrandtDistortion(px, py, u0, v0, fixed_distortion_coeffs); @@ -523,7 +523,7 @@ class vpXmlParserCamera::Impl cam_tmp.initProjWithKannalaBrandtDistortion(px, py, u0, v0, distortion_coeffs); } else { - vpERROR_TRACE("projection model type doesn't match with any known model !"); + std::cout << "Warning: projection model type doesn't match with any known model !" << std::endl; return SEQUENCE_ERROR; } @@ -625,16 +625,16 @@ class vpXmlParserCamera::Impl int nbCamera = 0; for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) { - if (node.type() != pugi::node_element) - continue; - - if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) { - prop = CODE_XML_OTHER; - } + if (node.type() == pugi::node_element) { + if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) { + prop = CODE_XML_OTHER; + } - if (prop == CODE_XML_CAMERA) { - if (SEQUENCE_OK == read_camera(node, cam_name, projModel, im_width, im_height, subsampl_width, subsampl_height, verbose)) - nbCamera++; + if (prop == CODE_XML_CAMERA) { + if (SEQUENCE_OK == read_camera(node, cam_name, projModel, im_width, im_height, subsampl_width, subsampl_height, verbose)) { + ++nbCamera; + } + } } } @@ -710,55 +710,55 @@ class vpXmlParserCamera::Impl vpXmlCodeSequenceType back = SEQUENCE_OK; for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) { - if (node.type() != pugi::node_element) - continue; - if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) { - prop = CODE_XML_OTHER; - back = SEQUENCE_ERROR; - } + if (node.type() == pugi::node_element) { + if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) { + prop = CODE_XML_OTHER; + back = SEQUENCE_ERROR; + } - switch (prop) { - case CODE_XML_CAMERA_NAME: - camera_name_tmp = node.text().as_string(); - break; - - case CODE_XML_WIDTH: - image_width_tmp = node.text().as_uint(); - break; - - case CODE_XML_HEIGHT: - image_height_tmp = node.text().as_uint(); - break; - - case CODE_XML_SUBSAMPLING_WIDTH: - subsampling_width_tmp = node.text().as_uint(); - break; - - case CODE_XML_SUBSAMPLING_HEIGHT: - subsampling_height_tmp = node.text().as_uint(); - break; - - case CODE_XML_MODEL: - break; - - case CODE_XML_ADDITIONAL_INFO: - break; - - case CODE_XML_BAD: - case CODE_XML_OTHER: - case CODE_XML_CAMERA: - case CODE_XML_FULL_HEIGHT: - case CODE_XML_FULL_WIDTH: - case CODE_XML_MODEL_TYPE: - case CODE_XML_U0: - case CODE_XML_V0: - case CODE_XML_PX: - case CODE_XML_PY: - case CODE_XML_KUD: - case CODE_XML_KDU: - default: - back = SEQUENCE_ERROR; - break; + switch (prop) { + case CODE_XML_CAMERA_NAME: + camera_name_tmp = node.text().as_string(); + break; + + case CODE_XML_WIDTH: + image_width_tmp = node.text().as_uint(); + break; + + case CODE_XML_HEIGHT: + image_height_tmp = node.text().as_uint(); + break; + + case CODE_XML_SUBSAMPLING_WIDTH: + subsampling_width_tmp = node.text().as_uint(); + break; + + case CODE_XML_SUBSAMPLING_HEIGHT: + subsampling_height_tmp = node.text().as_uint(); + break; + + case CODE_XML_MODEL: + break; + + case CODE_XML_ADDITIONAL_INFO: + break; + + case CODE_XML_BAD: + case CODE_XML_OTHER: + case CODE_XML_CAMERA: + case CODE_XML_FULL_HEIGHT: + case CODE_XML_FULL_WIDTH: + case CODE_XML_MODEL_TYPE: + case CODE_XML_U0: + case CODE_XML_V0: + case CODE_XML_PX: + case CODE_XML_PY: + case CODE_XML_KUD: + case CODE_XML_KDU: + default: + back = SEQUENCE_ERROR; + break; + } } } if (!((cam_name == camera_name_tmp) && (im_width == image_width_tmp || im_width == 0) && @@ -863,131 +863,17 @@ class vpXmlParserCamera::Impl switch (camera.get_projModel()) { case vpCameraParameters::perspectiveProjWithoutDistortion: - // - node_model = node_camera.append_child(LABEL_XML_MODEL); - { - node_tmp = node_model.append_child(pugi::node_comment); - node_tmp.set_value("Projection model type"); - - //without_distortion - node_tmp = node_model.append_child(LABEL_XML_MODEL_TYPE); - node_tmp.append_child(pugi::node_pcdata).set_value(LABEL_XML_MODEL_WITHOUT_DISTORTION); - - node_tmp = node_model.append_child(pugi::node_comment); - node_tmp.set_value("Pixel ratio"); - // - node_tmp = node_model.append_child(LABEL_XML_PX); - node_tmp.append_child(pugi::node_pcdata).text() = camera.get_px(); - // - node_tmp = node_model.append_child(LABEL_XML_PY); - node_tmp.append_child(pugi::node_pcdata).text() = camera.get_py(); - - node_tmp = node_model.append_child(pugi::node_comment); - node_tmp.set_value("Principal point"); - - // - node_tmp = node_model.append_child(LABEL_XML_U0); - node_tmp.append_child(pugi::node_pcdata).text() = camera.get_u0(); - // - node_tmp = node_model.append_child(LABEL_XML_V0); - node_tmp.append_child(pugi::node_pcdata).text() = camera.get_v0(); - } + writeCameraWithoutDistortion(node_camera); break; case vpCameraParameters::perspectiveProjWithDistortion: - // - node_model = node_camera.append_child(LABEL_XML_MODEL); - { - node_tmp = node_model.append_child(pugi::node_comment); - node_tmp.set_value("Projection model type"); - //with_distortion - node_tmp = node_model.append_child(LABEL_XML_MODEL_TYPE); - node_tmp.append_child(pugi::node_pcdata).set_value(LABEL_XML_MODEL_WITH_DISTORTION); - - node_tmp = node_model.append_child(pugi::node_comment); - node_tmp.set_value("Pixel ratio"); - // - node_tmp = node_model.append_child(LABEL_XML_PX); - node_tmp.append_child(pugi::node_pcdata).text() = camera.get_px(); - // - node_tmp = node_model.append_child(LABEL_XML_PY); - node_tmp.append_child(pugi::node_pcdata).text() = camera.get_py(); - - node_tmp = node_model.append_child(pugi::node_comment); - node_tmp.set_value("Principal point"); - // - node_tmp = node_model.append_child(LABEL_XML_U0); - node_tmp.append_child(pugi::node_pcdata).text() = camera.get_u0(); - // - node_tmp = node_model.append_child(LABEL_XML_V0); - node_tmp.append_child(pugi::node_pcdata).text() = camera.get_v0(); - - // - node_tmp = node_model.append_child(pugi::node_comment); - node_tmp.set_value("Undistorted to distorted distortion parameter"); - node_tmp = node_model.append_child(LABEL_XML_KUD); - node_tmp.append_child(pugi::node_pcdata).text() = camera.get_kud(); - - // - node_tmp = node_model.append_child(pugi::node_comment); - node_tmp.set_value("Distorted to undistorted distortion parameter"); - node_tmp = node_model.append_child(LABEL_XML_KDU); - node_tmp.append_child(pugi::node_pcdata).text() = camera.get_kdu(); - } + writeCameraWithDistortion(node_camera); break; case vpCameraParameters::ProjWithKannalaBrandtDistortion: - // - node_model = node_camera.append_child(LABEL_XML_MODEL); - { - node_tmp = node_model.append_child(pugi::node_comment); - node_tmp.set_value("Projection model type"); - //with_KannalaBrandt_distortion - node_tmp = node_model.append_child(LABEL_XML_MODEL_TYPE); - node_tmp.append_child(pugi::node_pcdata).set_value(LABEL_XML_MODEL_WITH_KANNALA_BRANDT_DISTORTION); - - node_tmp = node_model.append_child(pugi::node_comment); - node_tmp.set_value("Pixel ratio"); - // - node_tmp = node_model.append_child(LABEL_XML_PX); - node_tmp.append_child(pugi::node_pcdata).text() = camera.get_px(); - // - node_tmp = node_model.append_child(LABEL_XML_PY); - node_tmp.append_child(pugi::node_pcdata).text() = camera.get_py(); - - node_tmp = node_model.append_child(pugi::node_comment); - node_tmp.set_value("Principal point"); - // - node_tmp = node_model.append_child(LABEL_XML_U0); - node_tmp.append_child(pugi::node_pcdata).text() = camera.get_u0(); - // - node_tmp = node_model.append_child(LABEL_XML_V0); - node_tmp.append_child(pugi::node_pcdata).text() = camera.get_v0(); - - //, , , , - std::vector distortion_coefs = camera.getKannalaBrandtDistortionCoefficients(); - - if (distortion_coefs.size() != 5) - std::cout << "Make sure to have 5 distortion coefficients for Kannala-Brandt distortions." << std::endl; - - node_tmp = node_model.append_child(pugi::node_comment); - node_tmp.set_value("Distortion coefficients"); - node_tmp = node_model.append_child(LABEL_XML_K1); - distortion_coefs.size() == 0 ? node_tmp.append_child(pugi::node_pcdata).text() = 0 - : node_tmp.append_child(pugi::node_pcdata).text() = distortion_coefs[0]; - node_tmp = node_model.append_child(LABEL_XML_K2); - distortion_coefs.size() <= 1 ? node_tmp.append_child(pugi::node_pcdata).text() = 0 - : node_tmp.append_child(pugi::node_pcdata).text() = distortion_coefs[1]; - node_tmp = node_model.append_child(LABEL_XML_K3); - distortion_coefs.size() <= 2 ? node_tmp.append_child(pugi::node_pcdata).text() = 0 - : node_tmp.append_child(pugi::node_pcdata).text() = distortion_coefs[2]; - node_tmp = node_model.append_child(LABEL_XML_K4); - distortion_coefs.size() <= 3 ? node_tmp.append_child(pugi::node_pcdata).text() = 0 - : node_tmp.append_child(pugi::node_pcdata).text() = distortion_coefs[3]; - node_tmp = node_model.append_child(LABEL_XML_K5); - distortion_coefs.size() <= 4 ? node_tmp.append_child(pugi::node_pcdata).text() = 0 - : node_tmp.append_child(pugi::node_pcdata).text() = distortion_coefs[4]; - } + writeCameraWithKannalaBrandt(node_camera); + break; + default: break; } return back; @@ -1124,6 +1010,156 @@ class vpXmlParserCamera::Impl void setWidth(unsigned int width) { image_width = width; } private: + /*! + Write perspective projection camera parameters without distortion in an XML Tree. + \param node_camera : XML pointer node, pointing on a camera node. + */ + void writeCameraWithoutDistortion(pugi::xml_node &node_camera) + { + pugi::xml_node node_model; + pugi::xml_node node_tmp; + // + node_model = node_camera.append_child(LABEL_XML_MODEL); + node_tmp = node_model.append_child(pugi::node_comment); + node_tmp.set_value("Projection model type"); + + //without_distortion + node_tmp = node_model.append_child(LABEL_XML_MODEL_TYPE); + node_tmp.append_child(pugi::node_pcdata).set_value(LABEL_XML_MODEL_WITHOUT_DISTORTION); + + node_tmp = node_model.append_child(pugi::node_comment); + node_tmp.set_value("Pixel ratio"); + // + node_tmp = node_model.append_child(LABEL_XML_PX); + node_tmp.append_child(pugi::node_pcdata).text() = camera.get_px(); + // + node_tmp = node_model.append_child(LABEL_XML_PY); + node_tmp.append_child(pugi::node_pcdata).text() = camera.get_py(); + + node_tmp = node_model.append_child(pugi::node_comment); + node_tmp.set_value("Principal point"); + + // + node_tmp = node_model.append_child(LABEL_XML_U0); + node_tmp.append_child(pugi::node_pcdata).text() = camera.get_u0(); + // + node_tmp = node_model.append_child(LABEL_XML_V0); + node_tmp.append_child(pugi::node_pcdata).text() = camera.get_v0(); + } + + /*! + Write perspective projection camera parameters with distortion in an XML Tree. + \param node_camera : XML pointer node, pointing on a camera node. + */ + void writeCameraWithDistortion(pugi::xml_node &node_camera) + { + pugi::xml_node node_model; + pugi::xml_node node_tmp; + // + node_model = node_camera.append_child(LABEL_XML_MODEL); + node_tmp = node_model.append_child(pugi::node_comment); + node_tmp.set_value("Projection model type"); + //with_distortion + node_tmp = node_model.append_child(LABEL_XML_MODEL_TYPE); + node_tmp.append_child(pugi::node_pcdata).set_value(LABEL_XML_MODEL_WITH_DISTORTION); + + node_tmp = node_model.append_child(pugi::node_comment); + node_tmp.set_value("Pixel ratio"); + // + node_tmp = node_model.append_child(LABEL_XML_PX); + node_tmp.append_child(pugi::node_pcdata).text() = camera.get_px(); + // + node_tmp = node_model.append_child(LABEL_XML_PY); + node_tmp.append_child(pugi::node_pcdata).text() = camera.get_py(); + + node_tmp = node_model.append_child(pugi::node_comment); + node_tmp.set_value("Principal point"); + // + node_tmp = node_model.append_child(LABEL_XML_U0); + node_tmp.append_child(pugi::node_pcdata).text() = camera.get_u0(); + // + node_tmp = node_model.append_child(LABEL_XML_V0); + node_tmp.append_child(pugi::node_pcdata).text() = camera.get_v0(); + + // + node_tmp = node_model.append_child(pugi::node_comment); + node_tmp.set_value("Undistorted to distorted distortion parameter"); + node_tmp = node_model.append_child(LABEL_XML_KUD); + node_tmp.append_child(pugi::node_pcdata).text() = camera.get_kud(); + + // + node_tmp = node_model.append_child(pugi::node_comment); + node_tmp.set_value("Distorted to undistorted distortion parameter"); + node_tmp = node_model.append_child(LABEL_XML_KDU); + node_tmp.append_child(pugi::node_pcdata).text() = camera.get_kdu(); + } + + /*! + Write perspective projection camera parameters with Kannala-Brandt distortion in an XML Tree. + \param node_camera : XML pointer node, pointing on a camera node. + */ + void writeCameraWithKannalaBrandt(pugi::xml_node &node_camera) + { + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + const unsigned int requiredNbCoeff = 5; + pugi::xml_node node_model; + pugi::xml_node node_tmp; + // + node_model = node_camera.append_child(LABEL_XML_MODEL); + node_tmp = node_model.append_child(pugi::node_comment); + node_tmp.set_value("Projection model type"); + //with_KannalaBrandt_distortion + node_tmp = node_model.append_child(LABEL_XML_MODEL_TYPE); + node_tmp.append_child(pugi::node_pcdata).set_value(LABEL_XML_MODEL_WITH_KANNALA_BRANDT_DISTORTION); + + node_tmp = node_model.append_child(pugi::node_comment); + node_tmp.set_value("Pixel ratio"); + // + node_tmp = node_model.append_child(LABEL_XML_PX); + node_tmp.append_child(pugi::node_pcdata).text() = camera.get_px(); + // + node_tmp = node_model.append_child(LABEL_XML_PY); + node_tmp.append_child(pugi::node_pcdata).text() = camera.get_py(); + + node_tmp = node_model.append_child(pugi::node_comment); + node_tmp.set_value("Principal point"); + // + node_tmp = node_model.append_child(LABEL_XML_U0); + node_tmp.append_child(pugi::node_pcdata).text() = camera.get_u0(); + // + node_tmp = node_model.append_child(LABEL_XML_V0); + node_tmp.append_child(pugi::node_pcdata).text() = camera.get_v0(); + + //, , , , + std::vector distortion_coefs = camera.getKannalaBrandtDistortionCoefficients(); + + if (distortion_coefs.size() != requiredNbCoeff) { + std::cout << "Make sure to have 5 distortion coefficients for Kannala-Brandt distortions." << std::endl; + } + + node_tmp = node_model.append_child(pugi::node_comment); + node_tmp.set_value("Distortion coefficients"); + node_tmp = node_model.append_child(LABEL_XML_K1); + distortion_coefs.size() == index_0 ? node_tmp.append_child(pugi::node_pcdata).text() = 0 + : node_tmp.append_child(pugi::node_pcdata).text() = distortion_coefs[index_0]; + node_tmp = node_model.append_child(LABEL_XML_K2); + distortion_coefs.size() <= index_1 ? node_tmp.append_child(pugi::node_pcdata).text() = 0 + : node_tmp.append_child(pugi::node_pcdata).text() = distortion_coefs[index_1]; + node_tmp = node_model.append_child(LABEL_XML_K3); + distortion_coefs.size() <= index_2 ? node_tmp.append_child(pugi::node_pcdata).text() = 0 + : node_tmp.append_child(pugi::node_pcdata).text() = distortion_coefs[index_2]; + node_tmp = node_model.append_child(LABEL_XML_K4); + distortion_coefs.size() <= index_3 ? node_tmp.append_child(pugi::node_pcdata).text() = 0 + : node_tmp.append_child(pugi::node_pcdata).text() = distortion_coefs[index_3]; + node_tmp = node_model.append_child(LABEL_XML_K5); + distortion_coefs.size() <= index_4 ? node_tmp.append_child(pugi::node_pcdata).text() = 0 + : node_tmp.append_child(pugi::node_pcdata).text() = distortion_coefs[index_4]; + } + vpCameraParameters camera; std::string camera_name; unsigned int image_width; diff --git a/modules/core/src/display/vpColor.cpp b/modules/core/src/display/vpColor.cpp index fb2bbe45dd..3e02c0a8d4 100644 --- a/modules/core/src/display/vpColor.cpp +++ b/modules/core/src/display/vpColor.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/core/src/display/vpDisplay.cpp b/modules/core/src/display/vpDisplay.cpp index a15b635f6d..90c61eda7b 100644 --- a/modules/core/src/display/vpDisplay.cpp +++ b/modules/core/src/display/vpDisplay.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/core/src/display/vpDisplay_impl.h b/modules/core/src/display/vpDisplay_impl.h index a9c202fdf3..ac6ecf450e 100644 --- a/modules/core/src/display/vpDisplay_impl.h +++ b/modules/core/src/display/vpDisplay_impl.h @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Display implementation. - * -*****************************************************************************/ + */ #include #include @@ -81,23 +79,30 @@ void vp_display_display_camera(const vpImage &I, const vpHomogeneousMatrix double size, const vpColor &color, unsigned int thickness) { // used by display + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + const unsigned int val_5 = 5; double halfSize = size / 2.0; vpPoint pt[5]; - pt[0].setWorldCoordinates(-halfSize, -halfSize, 0.0); - pt[1].setWorldCoordinates(halfSize, -halfSize, 0.0); - pt[2].setWorldCoordinates(halfSize, halfSize, 0.0); - pt[3].setWorldCoordinates(-halfSize, halfSize, 0.0); - pt[4].setWorldCoordinates(0.0, 0.0, -size); + pt[index_0].setWorldCoordinates(-halfSize, -halfSize, 0.0); + pt[index_1].setWorldCoordinates(halfSize, -halfSize, 0.0); + pt[index_2].setWorldCoordinates(halfSize, halfSize, 0.0); + pt[index_3].setWorldCoordinates(-halfSize, halfSize, 0.0); + pt[index_4].setWorldCoordinates(0.0, 0.0, -size); - for (int i = 0; i < 5; ++i) + for (unsigned int i = 0; i < val_5; ++i) { pt[i].track(cMo); + } vpImagePoint ip, ip_1, ip0; - vpMeterPixelConversion::convertPoint(cam, pt[4].p[0], pt[4].p[1], ip0); + vpMeterPixelConversion::convertPoint(cam, pt[index_4].p[index_0], pt[index_4].p[index_1], ip0); for (int i = 0; i < 4; ++i) { - vpMeterPixelConversion::convertPoint(cam, pt[i].p[0], pt[i].p[1], ip_1); - vpMeterPixelConversion::convertPoint(cam, pt[(i + 1) % 4].p[0], pt[(i + 1) % 4].p[1], ip); + vpMeterPixelConversion::convertPoint(cam, pt[i].p[index_0], pt[i].p[index_1], ip_1); + vpMeterPixelConversion::convertPoint(cam, pt[(i + index_1) % index_4].p[0], pt[(i + 1) % index_4].p[1], ip); vpDisplay::displayLine(I, ip_1, ip, color, thickness); vpDisplay::displayLine(I, ip0, ip_1, color, thickness); } diff --git a/modules/core/src/display/vpDisplay_rgba.cpp b/modules/core/src/display/vpDisplay_rgba.cpp index 9737ea386d..84af8b05d4 100644 --- a/modules/core/src/display/vpDisplay_rgba.cpp +++ b/modules/core/src/display/vpDisplay_rgba.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -325,15 +324,15 @@ void vpDisplay::displayDotLine(const vpImage &I, const std::list &I, const vpImagePoint ¢er, const double &coef1, @@ -371,16 +370,16 @@ void vpDisplay::displayEllipse(const vpImage &I, const vpImagePoint &cen The following example shows how to use for example this function to display the result of a tracking. \code - vpMeEllipse ellipse; - ... - vpDisplay::display(I); - ellipse.track(I); + vpMeEllipse ellipse; + ... + vpDisplay::display(I); + ellipse.track(I); - vpDisplay::displayEllipse(I, ellipse.getCenter(), - ellipse.get_nij()[0], ellipse.get_nij()[1], ellipse.get_nij()[2], - ellipse.getSmallestAngle(), ellipse.getHighestAngle(), - true, vpColor::orange, 1); - vpDisplay::flush(I); + vpDisplay::displayEllipse(I, ellipse.getCenter(), + ellipse.get_nij()[0], ellipse.get_nij()[1], ellipse.get_nij()[2], + ellipse.getSmallestAngle(), ellipse.getHighestAngle(), + true, vpColor::orange, 1); + vpDisplay::flush(I); \endcode */ void vpDisplay::displayEllipse(const vpImage &I, const vpImagePoint ¢er, const double &coef1, @@ -986,69 +985,69 @@ bool vpDisplay::getClickUp(const vpImage &I, vpMouseButton::vpMouseButto to \e false. Below you will find an example showing how to use this method. -\code -#include -#include -#include -#include -#include -#include - -int main() -{ - vpImage I(240, 320); // Create a black image - - vpDisplay *d; - -#if defined(VISP_HAVE_X11) - d = new vpDisplayX; -#elif defined(VISP_HAVE_GTK) - d = new vpDisplayGTK; -#elif defined(VISP_HAVE_GDI) - d = new vpDisplayGDI; -#elif defined(VISP_HAVE_D3D9) - d = new vpDisplayD3D; -#elif defined(HAVE_OPENCV_HIGHGUI) - d = new vpDisplayOpenCV; -#else - std::cout << "Sorry, no video device is available" << std::endl; - return -1; -#endif - - // Initialize the display with the image I. Display and image are - // now link together. -#ifdef VISP_HAVE_DISPLAY - d->init(I); -#endif - - // Set the display background with image I content - vpDisplay::display(I); + \code + #include + #include + #include + #include + #include + #include + + int main() + { + vpImage I(240, 320); // Create a black image + + vpDisplay *d; + + #if defined(VISP_HAVE_X11) + d = new vpDisplayX; + #elif defined(VISP_HAVE_GTK) + d = new vpDisplayGTK; + #elif defined(VISP_HAVE_GDI) + d = new vpDisplayGDI; + #elif defined(VISP_HAVE_D3D9) + d = new vpDisplayD3D; + #elif defined(HAVE_OPENCV_HIGHGUI) + d = new vpDisplayOpenCV; + #else + std::cout << "Sorry, no video device is available" << std::endl; + return -1; + #endif + + // Initialize the display with the image I. Display and image are + // now link together. + #ifdef VISP_HAVE_DISPLAY + d->init(I); + #endif + + // Set the display background with image I content + vpDisplay::display(I); - // Flush the foreground and background display - vpDisplay::flush(I); + // Flush the foreground and background display + vpDisplay::flush(I); - // Wait for keyboard event - std::cout << "Waiting a keyboard event..." << std::endl; - vpDisplay::getKeyboardEvent(I, true); - std::cout << "A keyboard event was detected" << std::endl; - - // Non blocking keyboard event loop - int cpt_event = 0; - std::cout << "Enter a non blocking keyboard event detection loop..." << std::endl; - do { - bool event = vpDisplay::getKeyboardEvent(I, false); - if (event) { - std::cout << "A keyboard event was detected" << std::endl; cpt_event ++; - } + // Wait for keyboard event + std::cout << "Waiting a keyboard event..." << std::endl; + vpDisplay::getKeyboardEvent(I, true); + std::cout << "A keyboard event was detected" << std::endl; + + // Non blocking keyboard event loop + int cpt_event = 0; + std::cout << "Enter a non blocking keyboard event detection loop..." << std::endl; + do { + bool event = vpDisplay::getKeyboardEvent(I, false); + if (event) { + std::cout << "A keyboard event was detected" << std::endl; cpt_event ++; + } - vpTime::wait(5); // wait 5 ms - } while(cpt_event < 5); + vpTime::wait(5); // wait 5 ms + } while(cpt_event < 5); -#ifdef VISP_HAVE_DISPLAY - delete d; -#endif -} -\endcode + #ifdef VISP_HAVE_DISPLAY + delete d; + #endif + } + \endcode */ bool vpDisplay::getKeyboardEvent(const vpImage &I, bool blocking) { @@ -1076,70 +1075,70 @@ bool vpDisplay::getKeyboardEvent(const vpImage &I, bool blocking) to \e false. Below you will find an example showing how to use this method. -\code -#include -#include -#include -#include -#include - -int main() -{ - vpImage I(240, 320); // Create a black image - - vpDisplay *d; - -#if defined(VISP_HAVE_X11) - d = new vpDisplayX; -#elif defined(VISP_HAVE_GTK) - d = new vpDisplayGTK; -#elif defined(VISP_HAVE_GDI) - d = new vpDisplayGDI; -#elif defined(VISP_HAVE_D3D9) - d = new vpDisplayD3D; -#elif defined(HAVE_OPENCV_HIGHGUI) - d = new vpDisplayOpenCV; -#else - std::cout << "Sorry, no video device is available" << std::endl; - return -1; -#endif - - // Initialize the display with the image I. Display and image are - // now link together. -#ifdef VISP_HAVE_DISPLAY - d->init(I); -#endif - - // Set the display background with image I content - vpDisplay::display(I); + \code + #include + #include + #include + #include + #include + + int main() + { + vpImage I(240, 320); // Create a black image + + vpDisplay *d; + + #if defined(VISP_HAVE_X11) + d = new vpDisplayX; + #elif defined(VISP_HAVE_GTK) + d = new vpDisplayGTK; + #elif defined(VISP_HAVE_GDI) + d = new vpDisplayGDI; + #elif defined(VISP_HAVE_D3D9) + d = new vpDisplayD3D; + #elif defined(HAVE_OPENCV_HIGHGUI) + d = new vpDisplayOpenCV; + #else + std::cout << "Sorry, no video device is available" << std::endl; + return -1; + #endif + + // Initialize the display with the image I. Display and image are + // now link together. + #ifdef VISP_HAVE_DISPLAY + d->init(I); + #endif + + // Set the display background with image I content + vpDisplay::display(I); - // Flush the foreground and background display - vpDisplay::flush(I); + // Flush the foreground and background display + vpDisplay::flush(I); - // Wait for keyboard event - std::cout << "Waiting a keyboard event..." << std::endl; - vpDisplay::getKeyboardEvent(I, true); - std::cout << "A keyboard event was detected" << std::endl; - - // Non blocking keyboard event loop - int cpt_event = 0; - std::string key; - std::cout << "Enter a non blocking keyboard event detection loop..." << std::endl; - do { - bool event = vpDisplay::getKeyboardEvent(I, key, false); - if (event) { - std::cout << "Key detected: " << key << std::endl; - cpt_event ++; - } + // Wait for keyboard event + std::cout << "Waiting a keyboard event..." << std::endl; + vpDisplay::getKeyboardEvent(I, true); + std::cout << "A keyboard event was detected" << std::endl; + + // Non blocking keyboard event loop + int cpt_event = 0; + std::string key; + std::cout << "Enter a non blocking keyboard event detection loop..." << std::endl; + do { + bool event = vpDisplay::getKeyboardEvent(I, key, false); + if (event) { + std::cout << "Key detected: " << key << std::endl; + cpt_event ++; + } - vpTime::wait(5); // wait 5 ms - } while(cpt_event < 5); + vpTime::wait(5); // wait 5 ms + } while(cpt_event < 5); -#ifdef VISP_HAVE_DISPLAY - delete d; -#endif -} -\endcode + #ifdef VISP_HAVE_DISPLAY + delete d; + #endif + } + \endcode */ bool vpDisplay::getKeyboardEvent(const vpImage &I, std::string &key, bool blocking) { @@ -1167,70 +1166,70 @@ bool vpDisplay::getKeyboardEvent(const vpImage &I, std::string &key, boo to \e false. Below you will find an example showing how to use this method. -\code -#include -#include -#include -#include -#include - -int main() -{ - vpImage I(240, 320); // Create a black image - - vpDisplay *d; - -#if defined(VISP_HAVE_X11) - d = new vpDisplayX; -#elif defined(VISP_HAVE_GTK) - d = new vpDisplayGTK; -#elif defined(VISP_HAVE_GDI) - d = new vpDisplayGDI; -#elif defined(VISP_HAVE_D3D9) - d = new vpDisplayD3D; -#elif defined(HAVE_OPENCV_HIGHGUI) - d = new vpDisplayOpenCV; -#else - std::cout << "Sorry, no video device is available" << std::endl; - return -1; -#endif - - // Initialize the display with the image I. Display and image are - // now link together. -#ifdef VISP_HAVE_DISPLAY - d->init(I); -#endif - - // Set the display background with image I content - vpDisplay::display(I); + \code + #include + #include + #include + #include + #include + + int main() + { + vpImage I(240, 320); // Create a black image + + vpDisplay *d; + + #if defined(VISP_HAVE_X11) + d = new vpDisplayX; + #elif defined(VISP_HAVE_GTK) + d = new vpDisplayGTK; + #elif defined(VISP_HAVE_GDI) + d = new vpDisplayGDI; + #elif defined(VISP_HAVE_D3D9) + d = new vpDisplayD3D; + #elif defined(HAVE_OPENCV_HIGHGUI) + d = new vpDisplayOpenCV; + #else + std::cout << "Sorry, no video device is available" << std::endl; + return -1; + #endif + + // Initialize the display with the image I. Display and image are + // now link together. + #ifdef VISP_HAVE_DISPLAY + d->init(I); + #endif + + // Set the display background with image I content + vpDisplay::display(I); - // Flush the foreground and background display - vpDisplay::flush(I); + // Flush the foreground and background display + vpDisplay::flush(I); - // Wait for keyboard event - std::cout << "Waiting a keyboard event..." << std::endl; - vpDisplay::getKeyboardEvent(I, true); - std::cout << "A keyboard event was detected" << std::endl; - - // Non blocking keyboard event loop - int cpt_event = 0; - char key[10]; - std::cout << "Enter a non blocking keyboard event detection loop..." << std::endl; - do { - bool event = vpDisplay::getKeyboardEvent(I, &key[Ø], false); - if (event) { - std::cout << "Key detected: " << key << std::endl; - cpt_event ++; - } + // Wait for keyboard event + std::cout << "Waiting a keyboard event..." << std::endl; + vpDisplay::getKeyboardEvent(I, true); + std::cout << "A keyboard event was detected" << std::endl; + + // Non blocking keyboard event loop + int cpt_event = 0; + char key[10]; + std::cout << "Enter a non blocking keyboard event detection loop..." << std::endl; + do { + bool event = vpDisplay::getKeyboardEvent(I, &key[Ø], false); + if (event) { + std::cout << "Key detected: " << key << std::endl; + cpt_event ++; + } - vpTime::wait(5); // wait 5 ms - } while(cpt_event < 5); + vpTime::wait(5); // wait 5 ms + } while(cpt_event < 5); -#ifdef VISP_HAVE_DISPLAY - delete d; -#endif -} -\endcode + #ifdef VISP_HAVE_DISPLAY + delete d; + #endif + } + \endcode */ bool vpDisplay::getKeyboardEvent(const vpImage &I, char *key, bool blocking) { diff --git a/modules/core/src/display/vpDisplay_uchar.cpp b/modules/core/src/display/vpDisplay_uchar.cpp index 3e887ec109..bcef5006bc 100644 --- a/modules/core/src/display/vpDisplay_uchar.cpp +++ b/modules/core/src/display/vpDisplay_uchar.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -326,15 +325,15 @@ void vpDisplay::displayDotLine(const vpImage &I, const std::list< The following example shows how to use for example this function to display the result of a tracking. \code - vpMeEllipse ellipse; - ... - vpDisplay::display(I); - ellipse.track(I); + vpMeEllipse ellipse; + ... + vpDisplay::display(I); + ellipse.track(I); - vpDisplay::displayEllipse(I, ellipse.getCenter(), - ellipse.get_nij()[0], ellipse.get_nij()[1], ellipse.get_nij()[2], - true, vpColor::orange, 1); - vpDisplay::flush(I); + vpDisplay::displayEllipse(I, ellipse.getCenter(), + ellipse.get_nij()[0], ellipse.get_nij()[1], ellipse.get_nij()[2], + true, vpColor::orange, 1); + vpDisplay::flush(I); \endcode */ void vpDisplay::displayEllipse(const vpImage &I, const vpImagePoint ¢er, const double &coef1, @@ -372,16 +371,16 @@ void vpDisplay::displayEllipse(const vpImage &I, const vpImagePoi The following example shows how to use for example this function to display the result of a tracking. \code - vpMeEllipse ellipse; - ... - vpDisplay::display(I); - ellipse.track(I); + vpMeEllipse ellipse; + ... + vpDisplay::display(I); + ellipse.track(I); - vpDisplay::displayEllipse(I, ellipse.getCenter(), - ellipse.get_nij()[0], ellipse.get_nij()[1], ellipse.get_nij()[2], - ellipse.getSmallestAngle(), ellipse.getHighestAngle(), - true, vpColor::orange, 1); - vpDisplay::flush(I); + vpDisplay::displayEllipse(I, ellipse.getCenter(), + ellipse.get_nij()[0], ellipse.get_nij()[1], ellipse.get_nij()[2], + ellipse.getSmallestAngle(), ellipse.getHighestAngle(), + true, vpColor::orange, 1); + vpDisplay::flush(I); \endcode */ void vpDisplay::displayEllipse(const vpImage &I, const vpImagePoint ¢er, const double &coef1, @@ -986,70 +985,70 @@ bool vpDisplay::getClickUp(const vpImage &I, vpMouseButton::vpMou to \e false. Below you will find an example showing how to use this method. -\code -#include -#include -#include -#include -#include -#include - -int main() -{ - vpImage I(240, 320); // Create a black image - - vpDisplay *d; - -#if defined(VISP_HAVE_X11) - d = new vpDisplayX; -#elif defined(VISP_HAVE_GTK) - d = new vpDisplayGTK; -#elif defined(VISP_HAVE_GDI) - d = new vpDisplayGDI; -#elif defined(VISP_HAVE_D3D9) - d = new vpDisplayD3D; -#elif defined(HAVE_OPENCV_HIGHGUI) - d = new vpDisplayOpenCV; -#else - std::cout << "Sorry, no video device is available" << std::endl; - return -1; -#endif - - // Initialize the display with the image I. Display and image are - // now link together. -#ifdef VISP_HAVE_DISPLAY - d->init(I); -#endif - - // Set the display background with image I content - vpDisplay::display(I); + \code + #include + #include + #include + #include + #include + #include + + int main() + { + vpImage I(240, 320); // Create a black image + + vpDisplay *d; + + #if defined(VISP_HAVE_X11) + d = new vpDisplayX; + #elif defined(VISP_HAVE_GTK) + d = new vpDisplayGTK; + #elif defined(VISP_HAVE_GDI) + d = new vpDisplayGDI; + #elif defined(VISP_HAVE_D3D9) + d = new vpDisplayD3D; + #elif defined(HAVE_OPENCV_HIGHGUI) + d = new vpDisplayOpenCV; + #else + std::cout << "Sorry, no video device is available" << std::endl; + return -1; + #endif + + // Initialize the display with the image I. Display and image are + // now link together. + #ifdef VISP_HAVE_DISPLAY + d->init(I); + #endif + + // Set the display background with image I content + vpDisplay::display(I); - // Flush the foreground and background display - vpDisplay::flush(I); + // Flush the foreground and background display + vpDisplay::flush(I); - // Wait for keyboard event - std::cout << "Waiting a keyboard event..." << std::endl; - vpDisplay::getKeyboardEvent(I, true); - std::cout << "A keyboard event was detected" << std::endl; - - // Non blocking keyboard event loop - int cpt_event = 0; - std::cout << "Enter a non blocking keyboard event detection loop..." << std::endl; - do { - bool event = vpDisplay::getKeyboardEvent(I, false); - if (event) { - std::cout << "A keyboard event was detected" << std::endl; - cpt_event ++; - } + // Wait for keyboard event + std::cout << "Waiting a keyboard event..." << std::endl; + vpDisplay::getKeyboardEvent(I, true); + std::cout << "A keyboard event was detected" << std::endl; + + // Non blocking keyboard event loop + int cpt_event = 0; + std::cout << "Enter a non blocking keyboard event detection loop..." << std::endl; + do { + bool event = vpDisplay::getKeyboardEvent(I, false); + if (event) { + std::cout << "A keyboard event was detected" << std::endl; + cpt_event ++; + } - vpTime::wait(5); // wait 5 ms - } while(cpt_event < 5); + vpTime::wait(5); // wait 5 ms + } while(cpt_event < 5); -#ifdef VISP_HAVE_DISPLAY - delete d; -#endif -} -\endcode + #ifdef VISP_HAVE_DISPLAY + delete d; + #endif + } + \endcode */ bool vpDisplay::getKeyboardEvent(const vpImage &I, bool blocking) { @@ -1077,70 +1076,70 @@ bool vpDisplay::getKeyboardEvent(const vpImage &I, bool blocking) to \e false. Below you will find an example showing how to use this method. -\code -#include -#include -#include -#include -#include - -int main() -{ - vpImage I(240, 320); // Create a black image - - vpDisplay *d; - -#if defined(VISP_HAVE_X11) - d = new vpDisplayX; -#elif defined(VISP_HAVE_GTK) - d = new vpDisplayGTK; -#elif defined(VISP_HAVE_GDI) - d = new vpDisplayGDI; -#elif defined(VISP_HAVE_D3D9) - d = new vpDisplayD3D; -#elif defined(HAVE_OPENCV_HIGHGUI) - d = new vpDisplayOpenCV; -#else - std::cout << "Sorry, no video device is available" << std::endl; - return -1; -#endif - - // Initialize the display with the image I. Display and image are - // now link together. -#ifdef VISP_HAVE_DISPLAY - d->init(I); -#endif - - // Set the display background with image I content - vpDisplay::display(I); + \code + #include + #include + #include + #include + #include + + int main() + { + vpImage I(240, 320); // Create a black image + + vpDisplay *d; + + #if defined(VISP_HAVE_X11) + d = new vpDisplayX; + #elif defined(VISP_HAVE_GTK) + d = new vpDisplayGTK; + #elif defined(VISP_HAVE_GDI) + d = new vpDisplayGDI; + #elif defined(VISP_HAVE_D3D9) + d = new vpDisplayD3D; + #elif defined(HAVE_OPENCV_HIGHGUI) + d = new vpDisplayOpenCV; + #else + std::cout << "Sorry, no video device is available" << std::endl; + return -1; + #endif + + // Initialize the display with the image I. Display and image are + // now link together. + #ifdef VISP_HAVE_DISPLAY + d->init(I); + #endif + + // Set the display background with image I content + vpDisplay::display(I); - // Flush the foreground and background display - vpDisplay::flush(I); + // Flush the foreground and background display + vpDisplay::flush(I); - // Wait for keyboard event - std::cout << "Waiting a keyboard event..." << std::endl; - vpDisplay::getKeyboardEvent(I, true); - std::cout << "A keyboard event was detected" << std::endl; - - // Non blocking keyboard event loop - int cpt_event = 0; - std::string key; - std::cout << "Enter a non blocking keyboard event detection loop..." << std::endl; - do { - bool event = vpDisplay::getKeyboardEvent(I, key, false); - if (event) { - std::cout << "Key detected: " << key << std::endl; - cpt_event ++; - } + // Wait for keyboard event + std::cout << "Waiting a keyboard event..." << std::endl; + vpDisplay::getKeyboardEvent(I, true); + std::cout << "A keyboard event was detected" << std::endl; + + // Non blocking keyboard event loop + int cpt_event = 0; + std::string key; + std::cout << "Enter a non blocking keyboard event detection loop..." << std::endl; + do { + bool event = vpDisplay::getKeyboardEvent(I, key, false); + if (event) { + std::cout << "Key detected: " << key << std::endl; + cpt_event ++; + } - vpTime::wait(5); // wait 5 ms - } while(cpt_event < 5); + vpTime::wait(5); // wait 5 ms + } while(cpt_event < 5); -#ifdef VISP_HAVE_DISPLAY - delete d; -#endif -} -\endcode + #ifdef VISP_HAVE_DISPLAY + delete d; + #endif + } + \endcode */ bool vpDisplay::getKeyboardEvent(const vpImage &I, std::string &key, bool blocking) { @@ -1168,70 +1167,70 @@ bool vpDisplay::getKeyboardEvent(const vpImage &I, std::string &k to \e false. Below you will find an example showing how to use this method. -\code -#include -#include -#include -#include -#include - -int main() -{ - vpImage I(240, 320); // Create a black image - - vpDisplay *d; - -#if defined(VISP_HAVE_X11) - d = new vpDisplayX; -#elif defined(VISP_HAVE_GTK) - d = new vpDisplayGTK; -#elif defined(VISP_HAVE_GDI) - d = new vpDisplayGDI; -#elif defined(VISP_HAVE_D3D9) - d = new vpDisplayD3D; -#elif defined(HAVE_OPENCV_HIGHGUI) - d = new vpDisplayOpenCV; -#else - std::cout << "Sorry, no video device is available" << std::endl; - return -1; -#endif - - // Initialize the display with the image I. Display and image are - // now link together. -#ifdef VISP_HAVE_DISPLAY - d->init(I); -#endif - - // Set the display background with image I content - vpDisplay::display(I); + \code + #include + #include + #include + #include + #include + + int main() + { + vpImage I(240, 320); // Create a black image + + vpDisplay *d; + + #if defined(VISP_HAVE_X11) + d = new vpDisplayX; + #elif defined(VISP_HAVE_GTK) + d = new vpDisplayGTK; + #elif defined(VISP_HAVE_GDI) + d = new vpDisplayGDI; + #elif defined(VISP_HAVE_D3D9) + d = new vpDisplayD3D; + #elif defined(HAVE_OPENCV_HIGHGUI) + d = new vpDisplayOpenCV; + #else + std::cout << "Sorry, no video device is available" << std::endl; + return -1; + #endif + + // Initialize the display with the image I. Display and image are + // now link together. + #ifdef VISP_HAVE_DISPLAY + d->init(I); + #endif + + // Set the display background with image I content + vpDisplay::display(I); - // Flush the foreground and background display - vpDisplay::flush(I); + // Flush the foreground and background display + vpDisplay::flush(I); - // Wait for keyboard event - std::cout << "Waiting a keyboard event..." << std::endl; - vpDisplay::getKeyboardEvent(I, true); - std::cout << "A keyboard event was detected" << std::endl; - - // Non blocking keyboard event loop - int cpt_event = 0; - char key[10]; - std::cout << "Enter a non blocking keyboard event detection loop..." << std::endl; - do { - bool event = vpDisplay::getKeyboardEvent(I, &key[0], false); - if (event) { - std::cout << "Key detected: " << key << std::endl; - cpt_event ++; - } + // Wait for keyboard event + std::cout << "Waiting a keyboard event..." << std::endl; + vpDisplay::getKeyboardEvent(I, true); + std::cout << "A keyboard event was detected" << std::endl; + + // Non blocking keyboard event loop + int cpt_event = 0; + char key[10]; + std::cout << "Enter a non blocking keyboard event detection loop..." << std::endl; + do { + bool event = vpDisplay::getKeyboardEvent(I, &key[0], false); + if (event) { + std::cout << "Key detected: " << key << std::endl; + cpt_event ++; + } - vpTime::wait(5); // wait 5 ms - } while(cpt_event < 5); + vpTime::wait(5); // wait 5 ms + } while(cpt_event < 5); -#ifdef VISP_HAVE_DISPLAY - delete d; -#endif -} -\endcode + #ifdef VISP_HAVE_DISPLAY + delete d; + #endif + } + \endcode */ bool vpDisplay::getKeyboardEvent(const vpImage &I, char *key, bool blocking) { diff --git a/modules/core/src/display/vpFeatureDisplay.cpp b/modules/core/src/display/vpFeatureDisplay.cpp index 818e4ce2f1..fe0b523024 100644 --- a/modules/core/src/display/vpFeatureDisplay.cpp +++ b/modules/core/src/display/vpFeatureDisplay.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -41,9 +40,6 @@ // display #include -// Debug trace -#include - // math #include @@ -151,12 +147,17 @@ void vpFeatureDisplay::displayEllipse(double x, double y, double n20, double n11 { vpImagePoint center; double n20_p, n11_p, n02_p; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; vpCircle circle; - circle.p[0] = x; - circle.p[1] = y; - circle.p[2] = n20; - circle.p[3] = n11; - circle.p[4] = n02; + circle.p[index_0] = x; + circle.p[index_1] = y; + circle.p[index_2] = n20; + circle.p[index_3] = n11; + circle.p[index_4] = n02; vpMeterPixelConversion::convertEllipse(cam, circle, center, n20_p, n11_p, n02_p); vpDisplay::displayEllipse(I, center, n20_p, n11_p, n02_p, true, color, thickness); @@ -264,12 +265,17 @@ void vpFeatureDisplay::displayEllipse(double x, double y, double n20, double n11 { vpImagePoint center; double n20_p, n11_p, n02_p; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; vpCircle circle; - circle.p[0] = x; - circle.p[1] = y; - circle.p[2] = n20; - circle.p[3] = n11; - circle.p[4] = n02; + circle.p[index_0] = x; + circle.p[index_1] = y; + circle.p[index_2] = n20; + circle.p[index_3] = n11; + circle.p[index_4] = n02; vpMeterPixelConversion::convertEllipse(cam, circle, center, n20_p, n11_p, n02_p); vpDisplay::displayEllipse(I, center, n20_p, n11_p, n02_p, true, color, thickness); diff --git a/modules/core/src/framegrabber/vpFrameGrabber.cpp b/modules/core/src/framegrabber/vpFrameGrabber.cpp index 1c03c736ce..48c7d012a2 100644 --- a/modules/core/src/framegrabber/vpFrameGrabber.cpp +++ b/modules/core/src/framegrabber/vpFrameGrabber.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/core/src/image/private/vpImageConvert_impl.h b/modules/core/src/image/private/vpImageConvert_impl.h index 8258f50b84..da2188b86a 100644 --- a/modules/core/src/image/private/vpImageConvert_impl.h +++ b/modules/core/src/image/private/vpImageConvert_impl.h @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,16 +29,15 @@ * * Description: * Image conversion tools. - * -*****************************************************************************/ + */ /*! \file vpImageConvert_impl.h \brief Image conversion tools */ -#ifndef _vpIMAGECONVERT_impl_H_ -#define _vpIMAGECONVERT_impl_H_ +#ifndef VP_IMAGE_CONVERT_IMPL_H +#define VP_IMAGE_CONVERT_IMPL_H #if defined(VISP_HAVE_OPENMP) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) #include @@ -57,7 +55,7 @@ BEGIN_VISP_NAMESPACE \param[in] src_depth : Input float depth image. \param[out] dest_depth : Output grayscale depth image. */ -void vp_createDepthHistogram(const vpImage &src_depth, vpImage &dest_depth) + void vp_createDepthHistogram(const vpImage &src_depth, vpImage &dest_depth) { uint32_t histogram[0x10000]; memset(histogram, 0, sizeof(histogram)); diff --git a/modules/core/src/image/vpCannyEdgeDetection.cpp b/modules/core/src/image/vpCannyEdgeDetection.cpp index 78c26922fa..4ca3b1083e 100644 --- a/modules/core/src/image/vpCannyEdgeDetection.cpp +++ b/modules/core/src/image/vpCannyEdgeDetection.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -300,7 +299,7 @@ vpCannyEdgeDetection::computeFilteringAndGradient(const vpImage & * \brief Get the interpolation weights and offsets. * * \param[in] gradientOrientation : The positive value of the angle of the edge, expressed in radians. - * Its value is between 0 and M_PIf radians. + * Its value is between 0 and M_PI_FLOAT radians. * \param[out] alpha : The weight of the first point used for the interpolation. * \param[out] beta : The weight of the second point used for the interpolation. * \param[out] dRowGradAlpha : The offset along the row attached to the alpha weight. @@ -316,38 +315,38 @@ getInterpolWeightsAndOffsets(const float &gradientOrientation, ) { float thetaMin = 0.f; - if (gradientOrientation < M_PI_4f) { + if (gradientOrientation < M_PI_4_FLOAT) { // Angles between 0 and 45 deg rely on the horizontal and diagonal points dColGradAlpha = 1; dColGradBeta = 1; dRowGradAlpha = 0; dRowGradBeta = -1; } - else if ((gradientOrientation >= M_PI_4f) && (gradientOrientation < M_PI_2f)) { + else if ((gradientOrientation >= M_PI_4_FLOAT) && (gradientOrientation < M_PI_2_FLOAT)) { // Angles between 45 and 90 deg rely on the diagonal and vertical points - thetaMin = M_PI_4f; + thetaMin = M_PI_4_FLOAT; dColGradAlpha = 1; dColGradBeta = 0; dRowGradAlpha = -1; dRowGradBeta = -1; } - else if ((gradientOrientation >= M_PI_2f) && (gradientOrientation < (3.f * M_PI_4f))) { + else if ((gradientOrientation >= M_PI_2_FLOAT) && (gradientOrientation < (3.f * M_PI_4_FLOAT))) { // Angles between 90 and 135 deg rely on the vertical and diagonal points - thetaMin = M_PI_2f; + thetaMin = M_PI_2_FLOAT; dColGradAlpha = 0; dColGradBeta = -1; dRowGradAlpha = -1; dRowGradBeta = -1; } - else if ((gradientOrientation >= (3.f * M_PI_4f)) && (gradientOrientation < M_PIf)) { + else if ((gradientOrientation >= (3.f * M_PI_4_FLOAT)) && (gradientOrientation < M_PI_FLOAT)) { // Angles between 135 and 180 deg rely on the vertical and diagonal points - thetaMin = 3.f * M_PI_4f; + thetaMin = 3.f * M_PI_4_FLOAT; dColGradAlpha = -1; dColGradBeta = -1; dRowGradAlpha = -1; dRowGradBeta = 0; } - beta = (gradientOrientation - thetaMin) / M_PI_4f; + beta = (gradientOrientation - thetaMin) / M_PI_4_FLOAT; alpha = 1.f - beta; } @@ -379,7 +378,7 @@ getManhattanGradient(const vpImage &dIx, const vpImage &dIy, const } /** - * @brief Get the gradient orientation, expressed in radians, between 0 and M_PIf radians. + * @brief Get the gradient orientation, expressed in radians, between 0 and M_PI_FLOAT radians. * If the gradient orientation is negative, we add M_PI radians in * order to keep the same orientation but in the positive direction. * @@ -397,14 +396,14 @@ getGradientOrientation(const vpImage &dIx, const vpImage &dIy, con float dy = dIy[row][col]; if (std::abs(dx) < std::numeric_limits::epsilon()) { - gradientOrientation = M_PI_2f; + gradientOrientation = M_PI_2_FLOAT; } else { // -dy because the y-axis of the image is oriented towards the bottom of the screen // while we later work with a y-axis oriented towards the top when getting the theta quadrant. gradientOrientation = static_cast(std::atan2(-dy, dx)); if (gradientOrientation < 0.f) { - gradientOrientation += M_PIf; // + M_PI in order to be between 0 and M_PIf + gradientOrientation += M_PI_FLOAT; // + M_PI in order to be between 0 and M_PI_FLOAT } } return gradientOrientation; @@ -514,8 +513,9 @@ vpCannyEdgeDetection::recursiveSearchForStrongEdge(const std::pair(I.bitmap), I.getWidth() * 4, - reinterpret_cast(I_blur.bitmap), I_blur.getWidth() * 4); + const unsigned int rgba_size = 4; + SimdGaussianBlurRun(m_funcPtrRGBa, reinterpret_cast(I.bitmap), I.getWidth() * rgba_size, + reinterpret_cast(I_blur.bitmap), I_blur.getWidth() * rgba_size); } else { vpImageConvert::split(I, &m_red, &m_green, &m_blue); diff --git a/modules/core/src/image/vpImageCircle.cpp b/modules/core/src/image/vpImageCircle.cpp index afb49a7816..9ea2714dad 100644 --- a/modules/core/src/image/vpImageCircle.cpp +++ b/modules/core/src/image/vpImageCircle.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -51,10 +51,12 @@ vpImageCircle::vpImageCircle(const vpImagePoint ¢er, const float &radius) #ifdef HAVE_OPENCV_CORE vpImageCircle::vpImageCircle(const cv::Vec3f &vec) - : m_center(vec[1], vec[0]) - , m_radius(vec[2]) { - + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + m_center = vpImagePoint(vec[index_1], vec[index_0]); + m_radius = vec[index_2]; } #endif @@ -78,7 +80,7 @@ void computeIntersectionsLeftBorder(const float &u_c, const float &umin_roi, con float theta_min = std::min(theta1, theta2); float theta_max = std::max(theta1, theta2); delta_theta = theta_max - theta_min; - if ((u_c < umin_roi) && (std::abs(delta_theta - (2 * M_PIf)) < (2.f * std::numeric_limits::epsilon()))) { + if ((u_c < umin_roi) && (std::abs(delta_theta - (2 * M_PI_FLOAT)) < (2.f * std::numeric_limits::epsilon()))) { delta_theta = 0.f; } } @@ -102,8 +104,8 @@ void computeIntersectionsRightBorder(const float &u_c, const float &umax_roi, co float theta2 = -1.f * theta1; float theta_min = std::min(theta1, theta2); float theta_max = std::max(theta1, theta2); - delta_theta = (2.f * M_PIf) - (theta_max - theta_min); - if ((u_c > umax_roi) && (std::abs(delta_theta - (2 * M_PIf)) < (2.f * std::numeric_limits::epsilon()))) { + delta_theta = (2.f * M_PI_FLOAT) - (theta_max - theta_min); + if ((u_c > umax_roi) && (std::abs(delta_theta - (2 * M_PI_FLOAT)) < (2.f * std::numeric_limits::epsilon()))) { delta_theta = 0.f; } } @@ -127,25 +129,25 @@ void computeIntersectionsTopBorder(const float &v_c, const float &vmin_roi, cons float theta2 = 0.f; if (theta1 >= 0.f) { - theta2 = M_PIf - theta1; + theta2 = M_PI_FLOAT - theta1; } else { - theta2 = -theta1 - M_PIf; + theta2 = -theta1 - M_PI_FLOAT; } float theta_min = std::min(theta1, theta2); float theta_max = std::max(theta1, theta2); if ((std::abs(theta_max - theta_min) * radius) < 1.f) { // Between the maximum and minimum theta there is less than 1 pixel of difference // It meens that the full circle is visible - delta_theta = 2.f * M_PIf; + delta_theta = 2.f * M_PI_FLOAT; } else if (theta1 > 0.f) { - delta_theta = (2.f * M_PIf) - (theta_max - theta_min); + delta_theta = (2.f * M_PI_FLOAT) - (theta_max - theta_min); } else { delta_theta = theta_max - theta_min; } - if ((v_c < vmin_roi) && (std::abs(delta_theta - (2 * M_PIf)) < (2.f * std::numeric_limits::epsilon()))) { + if ((v_c < vmin_roi) && (std::abs(delta_theta - (2 * M_PI_FLOAT)) < (2.f * std::numeric_limits::epsilon()))) { delta_theta = 0.f; } } @@ -169,25 +171,25 @@ void computeIntersBottomBorder(const float &v_c, const float &vmax_roi, const fl float theta2 = 0.f; if (theta1 >= 0.f) { - theta2 = M_PIf - theta1; + theta2 = M_PI_FLOAT - theta1; } else { - theta2 = -theta1 - M_PIf; + theta2 = -theta1 - M_PI_FLOAT; } float theta_min = std::min(theta1, theta2); float theta_max = std::max(theta1, theta2); if ((std::abs(theta_max - theta_min) * radius) < 1.f) { // Between the maximum and minimum theta there is less than 1 pixel of difference // It meens that the full circle is visible - delta_theta = 2.f * M_PIf; + delta_theta = 2.f * M_PI_FLOAT; } else if (theta1 > 0.f) { delta_theta = theta_max - theta_min; } else { - delta_theta = (2.f * M_PIf) - (theta_max - theta_min); + delta_theta = (2.f * M_PI_FLOAT) - (theta_max - theta_min); } - if ((v_c > vmax_roi) && (std::abs(delta_theta - (2 * M_PIf)) < (2.f * std::numeric_limits::epsilon()))) { + if ((v_c > vmax_roi) && (std::abs(delta_theta - (2 * M_PI_FLOAT)) < (2.f * std::numeric_limits::epsilon()))) { delta_theta = 0.f; } } @@ -218,10 +220,10 @@ void computePerpendicularAxesInters(const float &u_c, const float &v_c, const fl theta_u_cross = vpMath::getAngleBetweenMinPiAndPi(theta_u_cross); float theta_u_cross_2 = 0.f; if (theta_u_cross > 0) { - theta_u_cross_2 = M_PIf - theta_u_cross; + theta_u_cross_2 = M_PI_FLOAT - theta_u_cross; } else { - theta_u_cross_2 = -M_PIf - theta_u_cross; + theta_u_cross_2 = -M_PI_FLOAT - theta_u_cross; } // Computing the corresponding u-coordinates at which the u-axis is crossed float u_ucross = u_c + (radius * std::cos(theta_u_cross)); @@ -347,13 +349,13 @@ void computeIntersectionsTopRight(const float &u_c, const float &v_c, const floa delta_theta = theta_v_max - theta_u_min; if (delta_theta < 0) { // The arc cannot be negative - delta_theta += 2.f * M_PIf; + delta_theta += 2.f * M_PI_FLOAT; } } else if ((u_umin <= umax_roi) && (v_vmin >= vmin_roi) && (u_umax <= umax_roi) && (v_vmax >= vmin_roi)) { // The circle crosses twice each axis //Case crossing twice - delta_theta = (2 * M_PIf) - ((theta_u_min - theta_u_max) + (theta_v_min - theta_v_max)); + delta_theta = (2 * M_PI_FLOAT) - ((theta_u_min - theta_u_max) + (theta_v_min - theta_v_max)); } else if ((u_umin >= umax_roi) && (v_vmin >= vmin_roi) && (u_umax >= umax_roi) && (v_vmax >= vmin_roi)) { // The circle crosses the u-axis outside the roi @@ -453,13 +455,13 @@ void computeIntersectionsBottomRight(const float &u_c, const float &v_c, const f delta_theta = theta_u_min - theta_v_min; if (delta_theta < 0) { // An arc length cannot be negative it means that theta_u_max was comprise in the bottom left quadrant of the circle - delta_theta += 2.f * M_PIf; + delta_theta += 2.f * M_PI_FLOAT; } } else if ((u_umin <= umax_roi) && (u_umax <= umax_roi) && (v_vmin <= vmax_roi) && (v_vmax <= vmax_roi)) { // The circle crosses twice each axis //Case crossing twice - delta_theta = (2.f * M_PIf) - ((theta_v_min - theta_v_max) + (theta_u_max - theta_u_min)); + delta_theta = (2.f * M_PI_FLOAT) - ((theta_v_min - theta_v_max) + (theta_u_max - theta_u_min)); } else if ((u_umin > umax_roi) && (u_umax > umax_roi) && (v_vmin <= vmax_roi) && (v_vmax <= vmax_roi)) { // The circle crosses the u-axis outside the roi @@ -577,7 +579,7 @@ void computeIntersTopRightBottom(const float &u_c, const float &v_c, const float float u_umax_bottom = crossing_theta_u_max.second; if ((u_umax_top <= umax_roi) && (u_umax_bottom <= umax_roi) && (v_vmin >= vmin_roi) && (v_vmax <= vmax_roi)) { // case intersection top + right + bottom twice - delta_theta = (2.f * M_PIf) - ((theta_u_min_top - theta_u_max_top) + (theta_v_min - theta_v_max) + (theta_u_max_bottom - theta_u_min_bottom)); + delta_theta = (2.f * M_PI_FLOAT) - ((theta_u_min_top - theta_u_max_top) + (theta_v_min - theta_v_max) + (theta_u_max_bottom - theta_u_min_bottom)); } else if ((u_umin_top <= umax_roi) && (u_umax_top > umax_roi) && (v_vmin <= vmin_roi) && (u_umin_bottom <= umax_roi) && (u_umax_bottom > umax_roi) && (v_vmax >= vmax_roi)) { // case intersection top and bottom @@ -618,10 +620,10 @@ void computeIntersTopBottomOnly(const float &u_c, const float &v_c, const float theta_u_cross_top = vpMath::getAngleBetweenMinPiAndPi(theta_u_cross_top); float theta_u_cross_top_2 = 0.f; if (theta_u_cross_top > 0) { - theta_u_cross_top_2 = M_PIf - theta_u_cross_top; + theta_u_cross_top_2 = M_PI_FLOAT - theta_u_cross_top; } else { - theta_u_cross_top_2 = -M_PIf - theta_u_cross_top; + theta_u_cross_top_2 = -M_PI_FLOAT - theta_u_cross_top; } // Computing the corresponding u-coordinates at which the u-axis is crossed @@ -645,10 +647,10 @@ void computeIntersTopBottomOnly(const float &u_c, const float &v_c, const float theta_u_cross_bottom = vpMath::getAngleBetweenMinPiAndPi(theta_u_cross_bottom); float theta_u_cross_bottom_2 = 0.f; if (theta_u_cross_bottom > 0) { - theta_u_cross_bottom_2 = M_PIf - theta_u_cross_bottom; + theta_u_cross_bottom_2 = M_PI_FLOAT - theta_u_cross_bottom; } else { - theta_u_cross_bottom_2 = -M_PIf - theta_u_cross_bottom; + theta_u_cross_bottom_2 = -M_PI_FLOAT - theta_u_cross_bottom; } // Computing the corresponding u-coordinates at which the u-axis is crossed @@ -668,7 +670,7 @@ void computeIntersTopBottomOnly(const float &u_c, const float &v_c, const float // Computing the the length of the angular interval of the circle when it intersects // only with the top and bottom borders of the Region of Interest (RoI) - delta_theta = (2.f * M_PIf) - ((theta_u_cross_top_min - theta_u_cross_top_max) + (theta_u_cross_bottom_max - theta_u_cross_bottom_min)); + delta_theta = (2.f * M_PI_FLOAT) - ((theta_u_cross_top_min - theta_u_cross_top_max) + (theta_u_cross_bottom_max - theta_u_cross_bottom_min)); } /*! @@ -931,7 +933,7 @@ float vpImageCircle::computeAngularCoverageInRoI(const vpRect &roi, const float // Easy case // The circle has its center in the image and its radius is not too great // to make it fully contained in the RoI - delta_theta = 2.f * M_PIf; + delta_theta = 2.f * M_PI_FLOAT; } else if (touchBottomBorder && (!touchLeftBorder) && (!touchRightBorder) && (!touchTopBorder)) { // Touches/intersects only the bottom border of the RoI @@ -1002,14 +1004,14 @@ float vpImageCircle::computeAngularCoverageInRoI(const vpRect &roi, const float throw(vpException(vpException::fatalError, "This case should never happen. Please contact Inria to make fix the problem")); } - if ((delta_theta < 0) || (delta_theta >(2.f * M_PIf))) { // Needed since M_PIf is used - float rest = vpMath::modulo(delta_theta, 2.f * M_PIf); - if ((rest < roundingTolerance) && ((delta_theta < -M_PIf) || (delta_theta > M_PIf))) { - // If the angle is a negative multiple of 2.f * M_PIf we consider it to be 2.f * M_PIf - delta_theta = 2.f * M_PIf; + if ((delta_theta < 0) || (delta_theta >(2.f * M_PI_FLOAT))) { // Needed since M_PI_FLOAT is used + float rest = vpMath::modulo(delta_theta, 2.f * M_PI_FLOAT); + if ((rest < roundingTolerance) && ((delta_theta < -M_PI_FLOAT) || (delta_theta > M_PI_FLOAT))) { + // If the angle is a negative multiple of 2.f * M_PI_FLOAT we consider it to be 2.f * M_PI_FLOAT + delta_theta = 2.f * M_PI_FLOAT; } else { - //Otherwise, it corresponds to delta_theta modulo 2.f * M_PIf + //Otherwise, it corresponds to delta_theta modulo 2.f * M_PI_FLOAT delta_theta = rest; } } @@ -1036,7 +1038,7 @@ void incrementIfIsInMask(const vpImage &mask, const int &width, const int } if (mask[y][x]) { // Increment only if the pixel value of the mask is true - count++; + ++count; } } }; @@ -1065,7 +1067,7 @@ unsigned int vpImageCircle::computePixelsInMask(const vpImage &mask) const #endif unsigned int count = 0; // Count the number of pixels of the circle whose value in the mask is true - const float thetaStop = M_PI_2f; + const float thetaStop = M_PI_2_FLOAT; float theta = 0; int x1 = 0, x2 = 0, x3 = 0, x4 = 0; int y1 = 0, y2 = 0, y3 = 0, y4 = 0; diff --git a/modules/core/src/image/vpImageConvert.cpp b/modules/core/src/image/vpImageConvert.cpp index 0ee31c2c16..cf26d19791 100644 --- a/modules/core/src/image/vpImageConvert.cpp +++ b/modules/core/src/image/vpImageConvert.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -337,15 +336,19 @@ void vpImageConvert::convert(const cv::Mat &src, vpImage &dest, bool fli dest.resize(static_cast(src.rows), static_cast(src.cols)); unsigned int destRows = dest.getRows(); unsigned int destCols = dest.getCols(); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; if (src.type() == CV_8UC4) { vpRGBa rgbaVal; for (unsigned int i = 0; i < destRows; ++i) for (unsigned int j = 0; j < destCols; ++j) { cv::Vec4b tmp = src.at(static_cast(i), static_cast(j)); - rgbaVal.R = tmp[2]; - rgbaVal.G = tmp[1]; - rgbaVal.B = tmp[0]; - rgbaVal.A = tmp[3]; + rgbaVal.R = tmp[index_2]; + rgbaVal.G = tmp[index_1]; + rgbaVal.B = tmp[index_0]; + rgbaVal.A = tmp[index_3]; if (flip) { dest[destRows - i - 1][j] = rgbaVal; } @@ -367,9 +370,9 @@ void vpImageConvert::convert(const cv::Mat &src, vpImage &dest, bool fli for (unsigned int i = 0; i < destRows; ++i) { for (unsigned int j = 0; j < destCols; ++j) { cv::Vec3b tmp = src.at(static_cast(i), static_cast(j)); - rgbaVal.R = tmp[2]; - rgbaVal.G = tmp[1]; - rgbaVal.B = tmp[0]; + rgbaVal.R = tmp[index_2]; + rgbaVal.G = tmp[index_1]; + rgbaVal.B = tmp[index_0]; if (flip) { dest[destRows - i - 1][j] = rgbaVal; } @@ -613,15 +616,18 @@ void vpImageConvert::convert(const cv::Mat &src, vpImage &dest, bool fli dest.resize(static_cast(src.rows), static_cast(src.cols)); unsigned int destRows = dest.getRows(); unsigned int destCols = dest.getCols(); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; if (src.type() == CV_32FC3) { vpRGBf rgbVal; for (unsigned int i = 0; i < destRows; ++i) for (unsigned int j = 0; j < destCols; ++j) { cv::Vec3f tmp = src.at(static_cast(i), static_cast(j)); - rgbVal.R = tmp[2]; - rgbVal.G = tmp[1]; - rgbVal.B = tmp[0]; + rgbVal.R = tmp[index_2]; + rgbVal.G = tmp[index_1]; + rgbVal.B = tmp[index_0]; if (flip) { dest[destRows - i - 1][j] = rgbVal; } @@ -1652,8 +1658,6 @@ void vpImageConvert::YCbCrToRGB(unsigned char *ycbcr, unsigned char *rgb, unsign val_g = *pt_ycbcr + vpImageConvert::vpCgb[*cbv] + vpImageConvert::vpCgr[*crv]; val_b = *pt_ycbcr + vpImageConvert::vpCbb[*cbv]; - 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 : 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. @@ -1712,8 +1716,6 @@ void vpImageConvert::YCbCrToRGBa(unsigned char *ycbcr, unsigned char *rgba, unsi val_g = *pt_ycbcr + vpImageConvert::vpCgb[*cbv] + vpImageConvert::vpCgr[*crv]; val_b = *pt_ycbcr + vpImageConvert::vpCbb[*cbv]; - 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 : 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. @@ -1799,8 +1801,6 @@ void vpImageConvert::YCrCbToRGB(unsigned char *ycrcb, unsigned char *rgb, unsign val_g = *pt_ycbcr + vpImageConvert::vpCgb[*cbv] + vpImageConvert::vpCgr[*crv]; val_b = *pt_ycbcr + vpImageConvert::vpCbb[*cbv]; - 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 : 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. @@ -1858,8 +1858,6 @@ void vpImageConvert::YCrCbToRGBa(unsigned char *ycrcb, unsigned char *rgba, unsi val_g = *pt_ycbcr + vpImageConvert::vpCgb[*cbv] + vpImageConvert::vpCgr[*crv]; val_b = *pt_ycbcr + vpImageConvert::vpCbb[*cbv]; - 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 : 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. @@ -1954,22 +1952,21 @@ void vpImageConvert::split(const vpImage &src, vpImage *p unsigned int width = src.getWidth(); unsigned char *input; unsigned char *dst; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; vpImage *tabChannel[4]; - /* incrsrc[0] = 0; //init - incrsrc[1] = 0; //step after the first used channel - incrsrc[2] = 0; //step after the second used channel - incrsrc[3] = 0; - incrsrc[4] = 0; - */ - tabChannel[0] = pR; - tabChannel[1] = pG; - tabChannel[2] = pB; - tabChannel[3] = pa; + tabChannel[index_0] = pR; + tabChannel[index_1] = pG; + tabChannel[index_2] = pB; + tabChannel[index_3] = pa; size_t i; /* ordre */ - for (unsigned int j = 0; j < 4; ++j) { + const unsigned int val_4 = 4; + for (unsigned int j = 0; j < val_4; ++j) { if (tabChannel[j] != nullptr) { if ((tabChannel[j]->getHeight() != height) || (tabChannel[j]->getWidth() != width)) { tabChannel[j]->resize(height, width); @@ -1990,7 +1987,7 @@ void vpImageConvert::split(const vpImage &src, vpImage *p ++dst; *dst = *input; input += 4; - dst++; + ++dst; *dst = *input; input += 4; ++dst; diff --git a/modules/core/src/image/vpImageConvert_hsv.cpp b/modules/core/src/image/vpImageConvert_hsv.cpp index 3d8ef91c9b..88888f5856 100644 --- a/modules/core/src/image/vpImageConvert_hsv.cpp +++ b/modules/core/src/image/vpImageConvert_hsv.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -57,8 +56,8 @@ BEGIN_VISP_NAMESPACE * \param[in] size : The image size or the number of pixels corresponding to the image width * height. * \param[in] step : Number of channels of the output color image; 3 for an RGB image, 4 for an RGBA image. */ -void vpImageConvert::HSV2RGB(const double *hue_, const double *saturation_, const double *value_, unsigned char *rgb, - unsigned int size, unsigned int step) + void vpImageConvert::HSV2RGB(const double *hue_, const double *saturation_, const double *value_, unsigned char *rgb, + unsigned int size, unsigned int step) { int size_ = static_cast(size); #if defined(_OPENMP) @@ -329,17 +328,21 @@ void vpImageConvert::RGB2HSV(const unsigned char *rgb, unsigned char *hue, unsig { int size_ = static_cast(size); std::vector h_scale(4); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; if (h_full) { - h_scale[0] = 42.5f; - h_scale[1] = 85.f; - h_scale[2] = 170.f; - h_scale[3] = 255.f; + h_scale[index_0] = 42.5f; + h_scale[index_1] = 85.f; + h_scale[index_2] = 170.f; + h_scale[index_3] = 255.f; } else { - h_scale[0] = 30.f; - h_scale[1] = 60.f; - h_scale[2] = 120.f; - h_scale[3] = 180.f; + h_scale[index_0] = 30.f; + h_scale[index_1] = 60.f; + h_scale[index_2] = 120.f; + h_scale[index_3] = 180.f; } #if defined(_OPENMP) #pragma omp parallel for @@ -379,17 +382,17 @@ void vpImageConvert::RGB2HSV(const unsigned char *rgb, unsigned char *hue, unsig float delta = max - min; if (vpMath::equal(red, max, std::numeric_limits::epsilon())) { - h = (h_scale[0] * (green - blue)) / delta; + h = (h_scale[index_0] * (green - blue)) / delta; } else if (vpMath::equal(green, max, std::numeric_limits::epsilon())) { - h = h_scale[1] + ((h_scale[0] * (blue - red)) / delta); + h = h_scale[index_1] + ((h_scale[index_0] * (blue - red)) / delta); } else { - h = h_scale[2] + ((h_scale[0] * (red - green)) / delta); + h = h_scale[index_2] + ((h_scale[index_0] * (red - green)) / delta); } if (h < 0.f) { - h += h_scale[3]; + h += h_scale[index_3]; } } diff --git a/modules/core/src/image/vpImageConvert_pcl.cpp b/modules/core/src/image/vpImageConvert_pcl.cpp index da5b771f12..11d6df52b7 100644 --- a/modules/core/src/image/vpImageConvert_pcl.cpp +++ b/modules/core/src/image/vpImageConvert_pcl.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -71,16 +70,19 @@ BEGIN_VISP_NAMESPACE * * \return The size of the point cloud. */ -int vpImageConvert::depthToPointCloud(const vpImage &depth_raw, float depth_scale, - const vpCameraParameters &cam_depth, - pcl::PointCloud::Ptr pointcloud, std::mutex *pointcloud_mutex, - const vpImage *depth_mask, float Z_min, float Z_max) + int vpImageConvert::depthToPointCloud(const vpImage &depth_raw, float depth_scale, + const vpCameraParameters &cam_depth, + pcl::PointCloud::Ptr pointcloud, std::mutex *pointcloud_mutex, + const vpImage *depth_mask, float Z_min, float Z_max) { int size = static_cast(depth_raw.getSize()); unsigned int width = depth_raw.getWidth(); unsigned int height = depth_raw.getHeight(); int pcl_size = 0; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; if (depth_mask) { if ((width != depth_mask->getWidth()) || (height != depth_mask->getHeight())) { @@ -105,11 +107,11 @@ int vpImageConvert::depthToPointCloud(const vpImage &depth_raw, float unsigned int i = (p - j) / width; vpPixelMeterConversion::convertPoint(cam_depth, j, i, x, y); vpColVector point_3D({ x * Z, y * Z, Z }); - if (point_3D[2] > Z_min) { + if (point_3D[index_2] > Z_min) { #if defined(_OPENMP) std::lock_guard lock(mutex); #endif - pointcloud->push_back(pcl::PointXYZ(point_3D[0], point_3D[1], point_3D[2])); + pointcloud->push_back(pcl::PointXYZ(point_3D[index_0], point_3D[index_1], point_3D[index_2])); } } } @@ -139,11 +141,11 @@ int vpImageConvert::depthToPointCloud(const vpImage &depth_raw, float unsigned int i = (p - j) / width; vpPixelMeterConversion::convertPoint(cam_depth, j, i, x, y); vpColVector point_3D({ x * Z, y * Z, Z, 1 }); - if (point_3D[2] >= 0.1) { + if (point_3D[index_2] >= 0.1) { #if defined(_OPENMP) std::lock_guard lock(mutex); #endif - pointcloud->push_back(pcl::PointXYZ(point_3D[0], point_3D[1], point_3D[2])); + pointcloud->push_back(pcl::PointXYZ(point_3D[index_0], point_3D[index_1], point_3D[index_2])); } } } @@ -189,6 +191,9 @@ int vpImageConvert::depthToPointCloud(const vpImage &color, const vpImag unsigned int width = depth_raw.getWidth(); unsigned int height = depth_raw.getHeight(); int pcl_size = 0; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; if (depth_mask) { if ((width != depth_mask->getWidth()) || (height != depth_mask->getHeight())) { @@ -213,18 +218,18 @@ int vpImageConvert::depthToPointCloud(const vpImage &color, const vpImag unsigned int i = (p - j) / width; vpPixelMeterConversion::convertPoint(cam_depth, j, i, x, y); vpColVector point_3D({ x * Z, y * Z, Z }); - if (point_3D[2] > Z_min) { + if (point_3D[index_2] > Z_min) { #if defined(_OPENMP) std::lock_guard lock(mutex); #endif #if PCL_VERSION_COMPARE(>=,1,14,1) - pointcloud->push_back(pcl::PointXYZRGB(point_3D[0], point_3D[1], point_3D[2], + pointcloud->push_back(pcl::PointXYZRGB(point_3D[index_0], point_3D[index_1], point_3D[index_2], color.bitmap[p].R, color.bitmap[p].G, color.bitmap[p].B)); #else pcl::PointXYZRGB pt(color.bitmap[p].R, color.bitmap[p].G, color.bitmap[p].B); - pt.x = point_3D[0]; - pt.y = point_3D[1]; - pt.z = point_3D[2]; + pt.x = point_3D[index_0]; + pt.y = point_3D[index_1]; + pt.z = point_3D[index_2]; pointcloud->push_back(pcl::PointXYZRGB(pt)); #endif } @@ -256,18 +261,18 @@ int vpImageConvert::depthToPointCloud(const vpImage &color, const vpImag unsigned int i = (p - j) / width; vpPixelMeterConversion::convertPoint(cam_depth, j, i, x, y); vpColVector point_3D({ x * Z, y * Z, Z, 1 }); - if (point_3D[2] >= 0.1) { + if (point_3D[index_2] >= 0.1) { #if defined(_OPENMP) std::lock_guard lock(mutex); #endif #if PCL_VERSION_COMPARE(>=,1,14,1) - pointcloud->push_back(pcl::PointXYZRGB(point_3D[0], point_3D[1], point_3D[2], + pointcloud->push_back(pcl::PointXYZRGB(point_3D[index_0], point_3D[index_1], point_3D[index_2], color.bitmap[p].R, color.bitmap[p].G, color.bitmap[p].B)); #else pcl::PointXYZRGB pt(color.bitmap[p].R, color.bitmap[p].G, color.bitmap[p].B); - pt.x = point_3D[0]; - pt.y = point_3D[1]; - pt.z = point_3D[2]; + pt.x = point_3D[index_0]; + pt.y = point_3D[index_1]; + pt.z = point_3D[index_2]; pointcloud->push_back(pcl::PointXYZRGB(pt)); #endif } diff --git a/modules/core/src/image/vpImageConvert_yuv.cpp b/modules/core/src/image/vpImageConvert_yuv.cpp index 799f61ff3b..1f714a5f32 100644 --- a/modules/core/src/image/vpImageConvert_yuv.cpp +++ b/modules/core/src/image/vpImageConvert_yuv.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -49,7 +48,8 @@ void vpSAT(int &c) c = 0; } else { - c = 255; + const unsigned int val_255 = 255; + c = val_255; } } }; @@ -73,23 +73,30 @@ void vpImageConvert::YUYVToRGBa(unsigned char *yuyv, unsigned char *rgba, unsign unsigned char *d; int w, h; int r, g, b, cr, cg, cb, y1, y2; + const unsigned int val_2 = 2; + const unsigned int val_88 = 88; + const unsigned int val_128 = 128; + const unsigned int val_183 = 183; + const unsigned int val_256 = 256; + const unsigned int val_359 = 359; + const unsigned int val_454 = 454; h = static_cast(height); w = static_cast(width); s = yuyv; d = rgba; while (h--) { - int c = w / 2; + int c = w / val_2; while (c--) { y1 = *s; ++s; - cb = ((*s - 128) * 454) / 256; - cg = (*s - 128) * 88; + cb = ((*s - val_128) * val_454) / val_256; + cg = (*s - val_128) * val_88; ++s; y2 = *s; ++s; - cr = ((*s - 128) * 359) / 256; - cg = (cg + ((*s - 128) * 183)) / 256; + cr = ((*s - val_128) * val_359) / val_256; + cg = (cg + ((*s - val_128) * val_183)) / val_256; ++s; r = y1 + cr; @@ -135,23 +142,30 @@ void vpImageConvert::YUYVToRGB(unsigned char *yuyv, unsigned char *rgb, unsigned unsigned char *d; int h, w; int r, g, b, cr, cg, cb, y1, y2; + const unsigned int val_2 = 2; + const unsigned int val_88 = 88; + const unsigned int val_128 = 128; + const unsigned int val_183 = 183; + const unsigned int val_256 = 256; + const unsigned int val_359 = 359; + const unsigned int val_454 = 454; h = static_cast(height); w = static_cast(width); s = yuyv; d = rgb; while (h--) { - int c = w / 2; + int c = w / val_2; while (c--) { y1 = *s; ++s; - cb = ((*s - 128) * 454) / 256; - cg = (*s - 128) * 88; + cb = ((*s - val_128) * val_454) / val_256; + cg = (*s - val_128) * val_88; ++s; y2 = *s; ++s; - cr = ((*s - 128) * 359) / 256; - cg = (cg + ((*s - 128) * 183)) / 256; + cr = ((*s - val_128) * val_359) / val_256; + cg = (cg + ((*s - val_128) * val_183)) / val_256; ++s; r = y1 + cr; @@ -211,15 +225,16 @@ void vpImageConvert::YUYVToGrey(unsigned char *yuyv, unsigned char *grey, unsign */ void vpImageConvert::YUV411ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int size) { + const unsigned int val_128 = 128; for (unsigned int i = size / 4; i; --i) { - int U = static_cast((*yuv - 128) * 0.354); + int U = static_cast((*yuv - val_128) * 0.354); ++yuv; int U5 = 5 * U; int Y0 = *yuv; ++yuv; int Y1 = *yuv; ++yuv; - int V = static_cast((*yuv - 128) * 0.707); + int V = static_cast((*yuv - val_128) * 0.707); ++yuv; int V2 = 2 * V; int Y2 = *yuv; @@ -307,13 +322,15 @@ 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); + const unsigned int val_2 = 2; + const unsigned int val_128 = 128; + for (unsigned int i = size / val_2; i; --i) { + int U = static_cast((*yuv - val_128) * 0.354); ++yuv; int U5 = 5 * U; int Y0 = *yuv; ++yuv; - int V = static_cast((*yuv - 128) * 0.707); + int V = static_cast((*yuv - val_128) * 0.707); ++yuv; int V2 = 2 * V; int Y1 = *yuv; @@ -363,7 +380,8 @@ void vpImageConvert::YUV422ToRGBa(unsigned char *yuv, unsigned char *rgba, unsig void vpImageConvert::YUV411ToGrey(unsigned char *yuv, unsigned char *grey, unsigned int size) { unsigned int i = 0, j = 0; - const unsigned int iterLimit = (size * 3) / 2; + const unsigned int val_2 = 2; + const unsigned int iterLimit = (size * 3) / val_2; while (j < iterLimit) { grey[i] = yuv[j + 1]; grey[i + 1] = yuv[j + 2]; @@ -388,13 +406,15 @@ 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); + const unsigned int val_2 = 2; + const unsigned int val_128 = 128; + for (unsigned int i = size / val_2; i; --i) { + int U = static_cast((*yuv - val_128) * 0.354); ++yuv; int U5 = 5 * U; int Y0 = *yuv; ++yuv; - int V = static_cast((*yuv - 128) * 0.707); + int V = static_cast((*yuv - val_128) * 0.707); ++yuv; int V2 = 2 * V; int Y1 = *yuv; @@ -463,15 +483,16 @@ void vpImageConvert::YUV422ToGrey(unsigned char *yuv, unsigned char *grey, unsig */ void vpImageConvert::YUV411ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int size) { + const unsigned int val_128 = 128; for (unsigned int i = size / 4; i; --i) { - int U = static_cast((*yuv - 128) * 0.354); + int U = static_cast((*yuv - val_128) * 0.354); ++yuv; int U5 = 5 * U; int Y0 = *yuv; ++yuv; int Y1 = *yuv; ++yuv; - int V = static_cast((*yuv - 128) * 0.707); + int V = static_cast((*yuv - val_128) * 0.707); ++yuv; int V2 = 2 * V; int Y2 = *yuv; @@ -555,18 +576,23 @@ void vpImageConvert::YUV420ToRGBa(unsigned char *yuv, unsigned char *rgba, unsig { int U, V, R, G, B, V2, U5, UV; int Y0, Y1, Y2, Y3; + const unsigned int val_2 = 2; + const unsigned int val_4 = 4; + const unsigned int val_5 = 5; + const unsigned int val_7 = 7; unsigned int size = width * height; unsigned char *iU = yuv + size; - unsigned char *iV = yuv + ((5 * size) / 4); - const unsigned int halfHeight = height / 2, halfWidth = width / 2; + unsigned char *iV = yuv + ((val_5 * size) / val_4); + const unsigned int halfHeight = height / val_2, halfWidth = width / val_2; + const unsigned int val_128 = 128; 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) - val_128) * 0.354); ++iU; - U5 = 5 * U; - V = static_cast(((*iV) - 128) * 0.707); + U5 = val_5 * U; + V = static_cast(((*iV) - val_128) * 0.707); ++iV; - V2 = 2 * V; + V2 = val_2 * V; UV = -U - V; Y0 = *yuv; ++yuv; @@ -609,7 +635,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 + (val_4 * width)) - val_7; //--- R = Y2 + V2; @@ -640,10 +666,10 @@ 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)) + 1; + rgba = (rgba - (val_4 * width)) + 1; } yuv += width; - rgba += 4 * width; + rgba += val_4 * width; } } @@ -659,18 +685,23 @@ void vpImageConvert::YUV420ToRGB(unsigned char *yuv, unsigned char *rgb, unsigne { int U, V, R, G, B, V2, U5, UV; int Y0, Y1, Y2, Y3; + const unsigned int val_2 = 2; + const unsigned int val_3 = 3; + const unsigned int val_4 = 4; + const unsigned int val_5 = 5; unsigned int size = width * height; unsigned char *iU = yuv + size; - unsigned char *iV = yuv + ((5 * size) / 4); - const unsigned int halfHeight = height / 2, halfWidth = width / 2; + unsigned char *iV = yuv + ((val_5 * size) / val_4); + const unsigned int halfHeight = height / val_2, halfWidth = width / val_2; + const unsigned int val_128 = 128; 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) - val_128) * 0.354); ++iU; - U5 = 5 * U; - V = static_cast(((*iV) - 128) * 0.707); + U5 = val_5 * U; + V = static_cast(((*iV) - val_128) * 0.707); ++iV; - V2 = 2 * V; + V2 = val_2 * V; UV = -U - V; Y0 = *yuv; ++yuv; @@ -711,7 +742,7 @@ void vpImageConvert::YUV420ToRGB(unsigned char *yuv, unsigned char *rgb, unsigne *rgb++ = static_cast(R); *rgb++ = static_cast(G); *rgb = static_cast(B); - rgb = rgb + ((3 * width) - 5); + rgb = rgb + ((val_3 * width) - val_5); //--- R = Y2 + V2; @@ -740,7 +771,7 @@ void vpImageConvert::YUV420ToRGB(unsigned char *yuv, unsigned char *rgb, unsigne *rgb++ = static_cast(R); *rgb++ = static_cast(G); *rgb = static_cast(B); - rgb = (rgb - (3 * width)) + 1; + rgb = (rgb - (val_3 * width)) + 1; } yuv += width; rgb += 3 * width; @@ -773,13 +804,14 @@ void vpImageConvert::YUV420ToGrey(unsigned char *yuv, unsigned char *grey, unsig */ void vpImageConvert::YUV444ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int size) { + const unsigned int val_128 = 128; for (unsigned int i = 0; i < size; ++i) { - int U = static_cast((*yuv - 128) * 0.354); + int U = static_cast((*yuv - val_128) * 0.354); ++yuv; int U5 = 5 * U; int Y = *yuv; ++yuv; - int V = static_cast((*yuv - 128) * 0.707); + int V = static_cast((*yuv - val_128) * 0.707); ++yuv; int V2 = 2 * V; int UV = -U - V; @@ -813,13 +845,14 @@ void vpImageConvert::YUV444ToRGBa(unsigned char *yuv, unsigned char *rgba, unsig */ void vpImageConvert::YUV444ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int size) { + const unsigned int val_128 = 128; for (unsigned int i = 0; i < size; ++i) { - int U = static_cast((*yuv - 128) * 0.354); + int U = static_cast((*yuv - val_128) * 0.354); ++yuv; int U5 = 5 * U; int Y = *yuv; ++yuv; - int V = static_cast((*yuv - 128) * 0.707); + int V = static_cast((*yuv - val_128) * 0.707); ++yuv; int V2 = 2 * V; int UV = -U - V; @@ -873,16 +906,21 @@ void vpImageConvert::YV12ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne { int U, V, R, G, B, V2, U5, UV; int Y0, Y1, Y2, Y3; + const unsigned int val_2 = 2; + const unsigned int val_4 = 4; + const unsigned int val_5 = 5; + const unsigned int val_7 = 7; unsigned int size = width * height; unsigned char *iV = yuv + size; - unsigned char *iU = yuv + ((5 * size) / 4); - const unsigned int halfHeight = height / 2, halfWidth = width / 2; + unsigned char *iU = yuv + ((val_5 * size) / val_4); + const unsigned int halfHeight = height / val_2, halfWidth = width / val_2; + const unsigned int val_128 = 128; 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) - val_128) * 0.354); ++iU; U5 = 5 * U; - V = static_cast(((*iV) - 128) * 0.707); + V = static_cast(((*iV) - val_128) * 0.707); ++iV; V2 = 2 * V; UV = -U - V; @@ -927,7 +965,7 @@ void vpImageConvert::YV12ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne *rgba++ = static_cast(G); *rgba++ = static_cast(B); *rgba = 0; - rgba = rgba + ((4 * width) - 7); + rgba = rgba + ((val_4 * width) - val_7); //--- R = Y2 + V2; @@ -958,10 +996,10 @@ void vpImageConvert::YV12ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne *rgba++ = static_cast(G); *rgba++ = static_cast(B); *rgba = vpRGBa::alpha_default; - rgba = (rgba - (4 * width)) + 1; + rgba = (rgba - (val_4 * width)) + 1; } yuv += width; - rgba += 4 * width; + rgba += val_4 * width; } } @@ -977,18 +1015,23 @@ void vpImageConvert::YV12ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned { int U, V, R, G, B, V2, U5, UV; int Y0, Y1, Y2, Y3; + const unsigned int val_2 = 2; + const unsigned int val_3 = 3; + const unsigned int val_4 = 4; + const unsigned int val_5 = 5; unsigned int size = width * height; unsigned char *iV = yuv + size; - unsigned char *iU = yuv + ((5 * size) / 4); - const unsigned int halfHeight = height / 2, halfWidth = width / 2; + unsigned char *iU = yuv + ((val_5 * size) / val_4); + const unsigned int halfHeight = height / val_2, halfWidth = width / val_2; + const unsigned int val_128 = 128; 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) - val_128) * 0.354); ++iU; - U5 = 5 * U; - V = static_cast(((*iV) - 128) * 0.707); + U5 = val_5 * U; + V = static_cast(((*iV) - val_128) * 0.707); ++iV; - V2 = 2 * V; + V2 = val_2 * V; UV = -U - V; Y0 = *yuv; ++yuv; @@ -1029,7 +1072,7 @@ void vpImageConvert::YV12ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned *rgb++ = static_cast(R); *rgb++ = static_cast(G); *rgb = static_cast(B); - rgb = rgb + ((3 * width) - 5); + rgb = rgb + ((val_3 * width) - 5); //--- R = Y2 + V2; @@ -1058,10 +1101,10 @@ void vpImageConvert::YV12ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned *rgb++ = static_cast(R); *rgb++ = static_cast(G); *rgb = static_cast(B); - rgb = (rgb - (3 * width)) + 1; + rgb = (rgb - (val_3 * width)) + 1; } yuv += width; - rgb += 3 * width; + rgb += val_3 * width; } } @@ -1079,18 +1122,27 @@ void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne { 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; + const unsigned int val_2 = 2; + const unsigned int val_3 = 3; + const unsigned int val_4 = 4; + const unsigned int val_5 = 5; + const unsigned int val_12 = 12; + const unsigned int val_15 = 15; + const unsigned int val_16 = 16; + const unsigned int val_17 = 17; unsigned int size = width * height; unsigned char *iV = yuv + size; - unsigned char *iU = yuv + ((17 * size) / 16); - const unsigned int quarterHeight = height / 4, quarterWidth = width / 4; + unsigned char *iU = yuv + ((val_17 * size) / val_16); + const unsigned int quarterHeight = height / val_4, quarterWidth = width / val_4; + const unsigned int val_128 = 128; 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) - val_128) * 0.354); ++iU; - U5 = 5 * U; - V = static_cast(((*iV) - 128) * 0.707); + U5 = val_5 * U; + V = static_cast(((*iV) - val_128) * 0.707); ++iV; - V2 = 2 * V; + V2 = val_2 * V; UV = -U - V; Y0 = *yuv; ++yuv; @@ -1099,7 +1151,7 @@ void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne Y2 = *yuv; ++yuv; Y3 = *yuv; - yuv = yuv + (width - 3); + yuv = yuv + (width - val_3); Y4 = *yuv; ++yuv; Y5 = *yuv; @@ -1107,7 +1159,7 @@ void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne Y6 = *yuv; ++yuv; Y7 = *yuv; - yuv = yuv + (width - 3); + yuv = yuv + (width - val_3); Y8 = *yuv; ++yuv; Y9 = *yuv; @@ -1115,7 +1167,7 @@ void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne Y10 = *yuv; ++yuv; Y11 = *yuv; - yuv = yuv + (width - 3); + yuv = yuv + (width - val_3); Y12 = *yuv; ++yuv; Y13 = *yuv; @@ -1123,7 +1175,7 @@ void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne Y14 = *yuv; ++yuv; Y15 = *yuv; - yuv = (yuv - (3 * width)) + 1; + yuv = (yuv - (val_3 * width)) + 1; // Original equations // R = Y + 1.402 V @@ -1187,7 +1239,7 @@ void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne *rgba++ = static_cast(G); *rgba++ = static_cast(B); *rgba = vpRGBa::alpha_default; - rgba = rgba + ((4 * width) - 15); + rgba = rgba + ((val_4 * width) - val_15); R = Y4 + V2; vpSAT(R); @@ -1247,7 +1299,7 @@ void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne *rgba++ = static_cast(G); *rgba++ = static_cast(B); *rgba = vpRGBa::alpha_default; - rgba = rgba + ((4 * width) - 15); + rgba = rgba + ((val_4 * width) - val_15); R = Y8 + V2; vpSAT(R); @@ -1307,7 +1359,7 @@ void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne *rgba++ = static_cast(G); *rgba++ = static_cast(B); *rgba = vpRGBa::alpha_default; - rgba = rgba + ((4 * width) - 15); + rgba = rgba + ((val_4 * width) - val_15); R = Y12 + V2; vpSAT(R); @@ -1367,10 +1419,10 @@ void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne *rgba++ = static_cast(G); *rgba++ = static_cast(B); *rgba = vpRGBa::alpha_default; - rgba = (rgba - (12 * width)) + 1; + rgba = (rgba - (val_12 * width)) + 1; } - yuv += 3 * width; - rgba += 12 * width; + yuv += val_3 * width; + rgba += val_12 * width; } } @@ -1385,18 +1437,26 @@ void vpImageConvert::YVU9ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned { 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; + const unsigned int val_2 = 2; + const unsigned int val_3 = 3; + const unsigned int val_4 = 4; + const unsigned int val_5 = 5; + const unsigned int val_9 = 9; + const unsigned int val_16 = 16; + const unsigned int val_17 = 17; unsigned int size = width * height; unsigned char *iV = yuv + size; - unsigned char *iU = yuv + ((17 * size) / 16); - const unsigned int quarterHeight = height / 4, quarterWidth = width / 4; + unsigned char *iU = yuv + ((val_17 * size) / val_16); + const unsigned int quarterHeight = height / val_4, quarterWidth = width / val_4; + const unsigned int val_128 = 128; 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 - val_128) * 0.354); ++iU; - U5 = 5 * U; - V = static_cast((*iV - 128) * 0.707); + U5 = val_5 * U; + V = static_cast((*iV - val_128) * 0.707); ++iV; - V2 = 2 * V; + V2 = val_2 * V; UV = -U - V; Y0 = *yuv; ++yuv; @@ -1405,7 +1465,7 @@ void vpImageConvert::YVU9ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned Y2 = *yuv; ++yuv; Y3 = *yuv; - yuv = yuv + (width - 3); + yuv = yuv + (width - val_3); Y4 = *yuv; ++yuv; Y5 = *yuv; @@ -1413,7 +1473,7 @@ void vpImageConvert::YVU9ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned Y6 = *yuv; ++yuv; Y7 = *yuv; - yuv = yuv + (width - 3); + yuv = yuv + (width - val_3); Y8 = *yuv; ++yuv; Y9 = *yuv; @@ -1421,7 +1481,7 @@ void vpImageConvert::YVU9ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned Y10 = *yuv; ++yuv; Y11 = *yuv; - yuv = yuv + (width - 3); + yuv = yuv + (width - val_3); Y12 = *yuv; ++yuv; Y13 = *yuv; @@ -1429,7 +1489,7 @@ void vpImageConvert::YVU9ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned Y14 = *yuv; ++yuv; Y15 = *yuv; - yuv = (yuv - (3 * width)) + 1; + yuv = (yuv - (val_3 * width)) + 1; // Original equations // R = Y + 1.402 V @@ -1489,7 +1549,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 + ((val_3 * width) - 11); R = Y4 + V2; vpSAT(R); @@ -1545,7 +1605,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 + ((val_3 * width) - 11); R = Y8 + V2; vpSAT(R); @@ -1601,7 +1661,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 + (val_3 * width)) - 11; R = Y12 + V2; vpSAT(R); @@ -1657,10 +1717,10 @@ void vpImageConvert::YVU9ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned *rgb++ = static_cast(R); *rgb++ = static_cast(G); *rgb++ = static_cast(B); - rgb = (rgb - (9 * width)) + 1; + rgb = (rgb - (val_9 * width)) + 1; } - yuv += 3 * width; - rgb += 9 * width; + yuv += val_3 * width; + rgb += val_9 * width; } } END_VISP_NAMESPACE diff --git a/modules/core/src/image/vpImageFilter.cpp b/modules/core/src/image/vpImageFilter.cpp index 54d6b177ad..5b5df75963 100644 --- a/modules/core/src/image/vpImageFilter.cpp +++ b/modules/core/src/image/vpImageFilter.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -45,8 +45,8 @@ BEGIN_VISP_NAMESPACE * \param[in] suf The suffix of the list. * \return std::string The list of available items. */ -std::string vpImageFilter::vpCannyBackendTypeList(const std::string &pref, const std::string &sep, - const std::string &suf) + std::string vpImageFilter::vpCannyBackendTypeList(const std::string &pref, const std::string &sep, + const std::string &suf) { std::string list(pref); for (unsigned int i = 0; i < (CANNY_COUNT_BACKEND - 1); ++i) { @@ -574,14 +574,15 @@ void vpImageFilter::getGaussXPyramidal(const vpImage &I, vpImage< { const unsigned int w = I.getWidth() / 2; const unsigned int height = I.getHeight(); + const unsigned int val_2 = 2; GI.resize(height, w); for (unsigned int i = 0; i < height; ++i) { GI[i][0] = I[i][0]; for (unsigned int j = 1; j < (w - 1); ++j) { - GI[i][j] = vpImageFilter::filterGaussXPyramidal(I, i, 2 * j); + GI[i][j] = vpImageFilter::filterGaussXPyramidal(I, i, val_2 * j); } - GI[i][w - 1] = I[i][(2 * w) - 1]; + GI[i][w - 1] = I[i][(val_2 * w) - 1]; } } @@ -589,14 +590,15 @@ void vpImageFilter::getGaussYPyramidal(const vpImage &I, vpImage< { const unsigned int h = I.getHeight() / 2; const unsigned int width = I.getWidth(); + const unsigned int val_2 = 2; GI.resize(h, width); for (unsigned int j = 0; j < width; ++j) { GI[0][j] = I[0][j]; for (unsigned int i = 1; i < (h - 1); ++i) { - GI[i][j] = vpImageFilter::filterGaussYPyramidal(I, 2 * i, j); + GI[i][j] = vpImageFilter::filterGaussYPyramidal(I, val_2 * i, j); } - GI[h - 1][j] = I[(2 * h) - 1][j]; + GI[h - 1][j] = I[(val_2 * h) - 1][j]; } } @@ -682,7 +684,8 @@ std::vector vpImageFilter::median(const vpImage &Isrc) std::vector meds(3); const int orderMeds[] = { 2, 1, 0 }; // To keep the order R, G, B const int orderCvChannels[] = { 0, 1, 2 }; // Because the order of the cv::Mat is B, G, R - for (unsigned int i = 0; i < 3; ++i) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { meds[orderMeds[i]] = median(channels[orderCvChannels[i]]); } return meds; diff --git a/modules/core/src/image/vpImagePoint.cpp b/modules/core/src/image/vpImagePoint.cpp index 5ad01f303a..2eb70923e1 100644 --- a/modules/core/src/image/vpImagePoint.cpp +++ b/modules/core/src/image/vpImagePoint.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/core/src/image/vpImageTools.cpp b/modules/core/src/image/vpImageTools.cpp index 5049092e0d..1a8aeca42e 100644 --- a/modules/core/src/image/vpImageTools.cpp +++ b/modules/core/src/image/vpImageTools.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -103,7 +102,6 @@ void vpImageTools::changeLUT(vpImage &I, unsigned char A, unsigne { // Test if input values are valid if (B <= A) { - vpERROR_TRACE("Bad gray levels"); throw(vpImageException(vpImageException::incorrectInitializationError, "Bad gray levels")); } unsigned char v; @@ -443,6 +441,10 @@ void vpImageTools::initUndistortMap(const vpCameraParameters &cam, unsigned int double r, scale; double theta, theta_d; double theta2, theta4, theta6, theta8; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; invpx = 1.0f / px; invpy = 1.0f / py; @@ -482,8 +484,8 @@ void vpImageTools::initUndistortMap(const vpCameraParameters &cam, unsigned int theta6 = theta2 * theta4; theta8 = vpMath::sqr(theta4); - theta_d = theta * (1 + (dist_coefs[0] * theta2) + (dist_coefs[1] * theta4) + (dist_coefs[2] * theta6) + - (dist_coefs[3] * theta8)); + theta_d = theta * (1 + (dist_coefs[index_0] * theta2) + (dist_coefs[index_1] * theta4) + (dist_coefs[index_2] * theta6) + + (dist_coefs[index_3] * theta8)); // --comment: scale eq (r == 0) 1.0 otherwise theta_d / r scale = (std::fabs(r) < std::numeric_limits::epsilon()) ? 1.0 : (theta_d / r); @@ -609,6 +611,35 @@ void vpImageTools::normalize(vpImage &I) } } +namespace +{ +/*! +* Get the interpolated value at a given location using the nearest points. +* \param I : The image to perform intepolation in. +* \param point : The image point. +*/ +double interpolationNearest(const vpImage &I, const vpImagePoint &point) +{ + int x1 = static_cast(floor(point.get_i())); + int x2 = static_cast(ceil(point.get_i())); + int y1 = static_cast(floor(point.get_j())); + int y2 = static_cast(ceil(point.get_j())); + double v1, v2; + if (x1 == x2) { + v1 = I(x1, y1); + v2 = I(x1, y2); + } + else { + v1 = ((x2 - point.get_i()) * I(x1, y1)) + ((point.get_i() - x1) * I(x2, y1)); + v2 = ((x2 - point.get_i()) * I(x1, y2)) + ((point.get_i() - x1) * I(x2, y2)); + } + if (y1 == y2) { + return v1; + } + return ((y2 - point.get_j()) * v1) + ((point.get_j() - y1) * v2); +} +} + /*! Get the interpolated value at a given location. \param I : The image to perform intepolation in. @@ -623,23 +654,7 @@ double vpImageTools::interpolate(const vpImage &I, const vpImageP case INTERPOLATION_NEAREST: return I(vpMath::round(point.get_i()), vpMath::round(point.get_j())); case INTERPOLATION_LINEAR: { - int x1 = static_cast(floor(point.get_i())); - int x2 = static_cast(ceil(point.get_i())); - int y1 = static_cast(floor(point.get_j())); - int y2 = static_cast(ceil(point.get_j())); - double v1, v2; - if (x1 == x2) { - v1 = I(x1, y1); - v2 = I(x1, y2); - } - else { - v1 = ((x2 - point.get_i()) * I(x1, y1)) + ((point.get_i() - x1) * I(x2, y1)); - v2 = ((x2 - point.get_i()) * I(x1, y2)) + ((point.get_i() - x1) * I(x2, y2)); - } - if (y1 == y2) { - return v1; - } - return ((y2 - point.get_j()) * v1) + ((point.get_j() - y1) * v2); + return interpolationNearest(I, point); } case INTERPOLATION_CUBIC: { throw vpException(vpException::notImplementedError, @@ -1003,15 +1018,18 @@ void vpImageTools::resizeSimdlib(const vpImage &Isrc, unsigned in bool vpImageTools::checkFixedPoint(unsigned int x, unsigned int y, const vpMatrix &T, bool affine) { - double a0 = T[0][0]; - double a1 = T[0][1]; - double a2 = T[0][2]; - double a3 = T[1][0]; - double a4 = T[1][1]; - double a5 = T[1][2]; - double a6 = affine ? 0.0 : T[2][0]; - double a7 = affine ? 0.0 : T[2][1]; - double a8 = affine ? 1.0 : T[2][2]; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + double a0 = T[index_0][index_0]; + double a1 = T[index_0][index_1]; + double a2 = T[index_0][index_2]; + double a3 = T[index_1][index_0]; + double a4 = T[index_1][index_1]; + double a5 = T[index_1][index_2]; + double a6 = affine ? 0.0 : T[index_2][index_0]; + double a7 = affine ? 0.0 : T[index_2][index_1]; + double a8 = affine ? 1.0 : T[index_2][index_2]; double w = (a6 * x) + (a7 * y) + a8; double x2 = ((a0 * x) + (a1 * y) + a2) / w; @@ -1113,12 +1131,18 @@ int vpImageTools::inRange(const unsigned char *hue, const unsigned char *saturat throw(vpImageException(vpImageException::notInitializedError, "Error in vpImageTools::inRange(): wrong values vector size (%d)", hsv_range.size())); } - unsigned char h_low = static_cast(hsv_range[0]); - unsigned char h_high = static_cast(hsv_range[1]); - unsigned char s_low = static_cast(hsv_range[2]); - unsigned char s_high = static_cast(hsv_range[3]); - unsigned char v_low = static_cast(hsv_range[4]); - unsigned char v_high = static_cast(hsv_range[5]); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + const unsigned int index_5 = 5; + unsigned char h_low = static_cast(hsv_range[index_0]); + unsigned char h_high = static_cast(hsv_range[index_1]); + unsigned char s_low = static_cast(hsv_range[index_2]); + unsigned char s_high = static_cast(hsv_range[index_3]); + unsigned char v_low = static_cast(hsv_range[index_4]); + unsigned char v_high = static_cast(hsv_range[index_5]); int size_ = static_cast(size); int cpt_in_range = 0; #if defined(_OPENMP) @@ -1167,12 +1191,18 @@ int vpImageTools::inRange(const unsigned char *hue, const unsigned char *saturat throw(vpImageException(vpImageException::notInitializedError, "Error in vpImageTools::inRange(): wrong values vector size (%d)", hsv_range.size())); } - unsigned char h_low = static_cast(hsv_range[0]); - unsigned char h_high = static_cast(hsv_range[1]); - unsigned char s_low = static_cast(hsv_range[2]); - unsigned char s_high = static_cast(hsv_range[3]); - unsigned char v_low = static_cast(hsv_range[4]); - unsigned char v_high = static_cast(hsv_range[5]); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + const unsigned int index_5 = 5; + unsigned char h_low = static_cast(hsv_range[index_0]); + unsigned char h_high = static_cast(hsv_range[index_1]); + unsigned char s_low = static_cast(hsv_range[index_2]); + unsigned char s_high = static_cast(hsv_range[index_3]); + unsigned char v_low = static_cast(hsv_range[index_4]); + unsigned char v_high = static_cast(hsv_range[index_5]); int size_ = static_cast(size); int cpt_in_range = 0; #if defined(_OPENMP) diff --git a/modules/core/src/image/vpRGBa.cpp b/modules/core/src/image/vpRGBa.cpp index 26ebd5d72b..ed70551af3 100644 --- a/modules/core/src/image/vpRGBa.cpp +++ b/modules/core/src/image/vpRGBa.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -40,7 +39,6 @@ */ #include -#include #include #include @@ -97,13 +95,16 @@ vpRGBa &vpRGBa::operator=(const vpRGBa &&v) vpRGBa &vpRGBa::operator=(const vpColVector &v) { if (v.getRows() != 4) { - vpERROR_TRACE("Bad vector dimension "); throw(vpException(vpException::dimensionError, "Bad vector dimension ")); } - R = static_cast(v[0]); - G = static_cast(v[1]); - B = static_cast(v[2]); - A = static_cast(v[3]); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + R = static_cast(v[index_0]); + G = static_cast(v[index_1]); + B = static_cast(v[index_2]); + A = static_cast(v[index_3]); return *this; } @@ -131,10 +132,14 @@ bool vpRGBa::operator!=(const vpRGBa &v) const { return ((R != v.R) || (G != v.G vpColVector vpRGBa::operator-(const vpRGBa &v) const { vpColVector n(4); // new color - n[0] = static_cast(R) - static_cast(v.R); - n[1] = static_cast(G) - static_cast(v.G); - n[2] = static_cast(B) - static_cast(v.B); - n[3] = static_cast(A) - static_cast(v.A); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + n[index_0] = static_cast(R) - static_cast(v.R); + n[index_1] = static_cast(G) - static_cast(v.G); + n[index_2] = static_cast(B) - static_cast(v.B); + n[index_3] = static_cast(A) - static_cast(v.A); return n; } @@ -162,10 +167,14 @@ vpRGBa vpRGBa::operator+(const vpRGBa &v) const vpColVector vpRGBa::operator-(const vpColVector &v) const { vpColVector n(4); // new color - n[0] = R - v[0]; - n[1] = G - v[1]; - n[2] = B - v[2]; - n[3] = A - v[3]; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + n[index_0] = R - v[index_0]; + n[index_1] = G - v[index_1]; + n[index_2] = B - v[index_2]; + n[index_3] = A - v[index_3]; return n; } @@ -177,10 +186,14 @@ vpColVector vpRGBa::operator-(const vpColVector &v) const vpColVector vpRGBa::operator+(const vpColVector &v) const { vpColVector n(4); // new color - n[0] = R + v[0]; - n[1] = G + v[1]; - n[2] = B + v[2]; - n[3] = A + v[3]; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + n[index_0] = R + v[index_0]; + n[index_1] = G + v[index_1]; + n[index_2] = B + v[index_2]; + n[index_3] = A + v[index_3]; return n; } @@ -192,10 +205,14 @@ vpColVector vpRGBa::operator+(const vpColVector &v) const vpColVector vpRGBa::operator*(const float &v) const { vpColVector n(4); - n[0] = R * v; - n[1] = G * v; - n[2] = B * v; - n[3] = A * v; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + n[index_0] = R * v; + n[index_1] = G * v; + n[index_2] = B * v; + n[index_3] = A * v; return n; } @@ -207,10 +224,14 @@ vpColVector vpRGBa::operator*(const float &v) const vpColVector vpRGBa::operator*(const double &v) const { vpColVector n(4); - n[0] = R * v; - n[1] = G * v; - n[2] = B * v; - n[3] = A * v; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + n[index_0] = R * v; + n[index_1] = G * v; + n[index_2] = B * v; + n[index_3] = A * v; return n; } diff --git a/modules/core/src/image/vpRGBf.cpp b/modules/core/src/image/vpRGBf.cpp index 1213ffe49c..646b905d69 100644 --- a/modules/core/src/image/vpRGBf.cpp +++ b/modules/core/src/image/vpRGBf.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -41,7 +40,6 @@ #include #include -#include #include #include @@ -93,13 +91,17 @@ vpRGBf &vpRGBf::operator=(const vpRGBf &&v) */ vpRGBf &vpRGBf::operator=(const vpColVector &v) { - if (v.getRows() != 3) { - vpERROR_TRACE("Bad vector dimension"); + const unsigned int rows_size = 3; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + + if (v.getRows() != rows_size) { throw(vpException(vpException::dimensionError, "Bad vector dimension")); } - R = static_cast(v[0]); - G = static_cast(v[1]); - B = static_cast(v[2]); + R = static_cast(v[index_0]); + G = static_cast(v[index_1]); + B = static_cast(v[index_2]); return *this; } @@ -142,9 +144,12 @@ bool vpRGBf::operator!=(const vpRGBf &v) const vpColVector vpRGBf::operator-(const vpRGBf &v) const { vpColVector n(3); // new color - n[0] = static_cast(R) - static_cast(v.R); - n[1] = static_cast(G) - static_cast(v.G); - n[2] = static_cast(B) - static_cast(v.B); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + n[index_0] = static_cast(R) - static_cast(v.R); + n[index_1] = static_cast(G) - static_cast(v.G); + n[index_2] = static_cast(B) - static_cast(v.B); return n; } @@ -171,9 +176,12 @@ vpRGBf vpRGBf::operator+(const vpRGBf &v) const vpColVector vpRGBf::operator-(const vpColVector &v) const { vpColVector n(3); // new color - n[0] = R - v[0]; - n[1] = G - v[1]; - n[2] = B - v[2]; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + n[index_0] = R - v[index_0]; + n[index_1] = G - v[index_1]; + n[index_2] = B - v[index_2]; return n; } @@ -185,9 +193,12 @@ vpColVector vpRGBf::operator-(const vpColVector &v) const vpColVector vpRGBf::operator+(const vpColVector &v) const { vpColVector n(3); // new color - n[0] = R + v[0]; - n[1] = G + v[1]; - n[2] = B + v[2]; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + n[index_0] = R + v[index_0]; + n[index_1] = G + v[index_1]; + n[index_2] = B + v[index_2]; return n; } @@ -199,9 +210,12 @@ vpColVector vpRGBf::operator+(const vpColVector &v) const vpColVector vpRGBf::operator*(float v) const { vpColVector n(3); - n[0] = R * v; - n[1] = G * v; - n[2] = B * v; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + n[index_0] = R * v; + n[index_1] = G * v; + n[index_2] = B * v; return n; } @@ -213,9 +227,12 @@ vpColVector vpRGBf::operator*(float v) const vpColVector vpRGBf::operator*(double v) const { vpColVector n(3); - n[0] = R * v; - n[1] = G * v; - n[2] = B * v; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + n[index_0] = R * v; + n[index_1] = G * v; + n[index_2] = B * v; return n; } diff --git a/modules/core/src/math/matrix/vpColVector.cpp b/modules/core/src/math/matrix/vpColVector.cpp index 564a502d20..7c0df736fc 100644 --- a/modules/core/src/math/matrix/vpColVector.cpp +++ b/modules/core/src/math/matrix/vpColVector.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -48,7 +48,6 @@ #include #include -#include #include #include #include @@ -80,7 +79,8 @@ vpTranslationVector vpColVector::operator+(const vpTranslationVector &t) const } vpTranslationVector s; - for (unsigned int i = 0; i < 3; ++i) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { s[i] = (*this)[i] + t[i]; } @@ -712,21 +712,25 @@ double vpColVector::stdev(const vpColVector &v, bool useBesselCorrection) vpMatrix vpColVector::skew(const vpColVector &v) { vpMatrix M; - if (v.getRows() != 3) { + const unsigned int rows_size = 3; + if (v.getRows() != rows_size) { throw(vpException(vpException::dimensionError, "Cannot compute skew vector of a non 3-dimension vector (%d)", v.getRows())); } M.resize(3, 3, false, false); - M[0][0] = 0; - M[0][1] = -v[2]; - M[0][2] = v[1]; - M[1][0] = v[2]; - M[1][1] = 0; - M[1][2] = -v[0]; - M[2][0] = -v[1]; - M[2][1] = v[0]; - M[2][2] = 0; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + M[index_0][index_0] = 0; + M[index_0][index_1] = -v[index_2]; + M[index_0][index_2] = v[index_1]; + M[index_1][index_0] = v[index_2]; + M[index_1][index_1] = 0; + M[index_1][index_2] = -v[index_0]; + M[index_2][index_0] = -v[index_1]; + M[index_2][index_1] = v[index_0]; + M[index_2][index_2] = 0; return M; } diff --git a/modules/core/src/math/matrix/vpEigenConversion.cpp b/modules/core/src/math/matrix/vpEigenConversion.cpp index 87e63b3736..3a179cbae3 100644 --- a/modules/core/src/math/matrix/vpEigenConversion.cpp +++ b/modules/core/src/math/matrix/vpEigenConversion.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -68,7 +67,7 @@ void eigen2visp(const Eigen::VectorXd &src, VISP_NAMESPACE_ADDRESSING vpColVecto #endif dst[static_cast(i)] = src(i); } -} + } void eigen2visp(const Eigen::RowVectorXd &src, VISP_NAMESPACE_ADDRESSING vpRowVector &dst) { @@ -82,7 +81,7 @@ void eigen2visp(const Eigen::RowVectorXd &src, VISP_NAMESPACE_ADDRESSING vpRowVe #endif dst[static_cast(i)] = src(i); } -} + } void visp2eigen(const VISP_NAMESPACE_ADDRESSING vpColVector &src, Eigen::VectorXd &dst) { dst = Eigen::VectorXd::Map(src.data, src.size()); } diff --git a/modules/core/src/math/matrix/vpMatrix.cpp b/modules/core/src/math/matrix/vpMatrix.cpp index fbdeaf14cf..58607ae527 100644 --- a/modules/core/src/math/matrix/vpMatrix.cpp +++ b/modules/core/src/math/matrix/vpMatrix.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -52,7 +51,6 @@ #include #include #include -#include #include #include #include @@ -957,13 +955,14 @@ vpTranslationVector vpMatrix::operator*(const vpTranslationVector &tv) const rowNum, colNum, tv.getRows(), tv.getCols())); } - for (unsigned int j = 0; j < 3; ++j) { + const unsigned int val_3 = 3; + for (unsigned int j = 0; j < val_3; ++j) { t_out[j] = 0; } - for (unsigned int j = 0; j < 3; ++j) { + for (unsigned int j = 0; j < val_3; ++j) { double tj = tv[j]; // optimization em 5/12/2006 - for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int i = 0; i < val_3; ++i) { t_out[i] += rowPtrs[i][j] * tj; } } diff --git a/modules/core/src/math/matrix/vpMatrix_cholesky.cpp b/modules/core/src/math/matrix/vpMatrix_cholesky.cpp index 3293cd86e1..e6252e8fd9 100644 --- a/modules/core/src/math/matrix/vpMatrix_cholesky.cpp +++ b/modules/core/src/math/matrix/vpMatrix_cholesky.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -42,9 +41,6 @@ #include #include -// Debug trace -#include - #if defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1 #include #endif diff --git a/modules/core/src/math/matrix/vpMatrix_covariance.cpp b/modules/core/src/math/matrix/vpMatrix_covariance.cpp index aed1ad5ad7..942d143f34 100644 --- a/modules/core/src/math/matrix/vpMatrix_covariance.cpp +++ b/modules/core/src/math/matrix/vpMatrix_covariance.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,7 @@ * * Description: * Covariance matrix computation. - * - * Authors: - * Aurelien Yol - * -*****************************************************************************/ + */ #include // std::fabs() #include // numeric_limits @@ -166,9 +161,12 @@ void vpMatrix::computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const // building Lp vpMatrix LpInv(6, 6); LpInv = 0; - LpInv[0][0] = -1.0; - LpInv[1][1] = -1.0; - LpInv[2][2] = -1.0; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + LpInv[index_0][index_0] = -1.0; + LpInv[index_1][index_1] = -1.0; + LpInv[index_2][index_2] = -1.0; vpTranslationVector ctoInit; @@ -179,7 +177,8 @@ void vpMatrix::computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const cMo.extract(thetau); vpColVector tu(3); - for (unsigned int i = 0; i < 3; ++i) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { tu[i] = thetau[i]; } @@ -195,13 +194,13 @@ void vpMatrix::computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const if ((theta / (2.0 * M_PI)) > std::numeric_limits::epsilon()) { // Computing [theta/2 u]_x vpColVector theta2u(3); - for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int i = 0; i < val_3; ++i) { theta2u[i] = tu[i] / 2.0; } vpMatrix theta2u_skew = vpColVector::skew(theta2u); vpColVector u(3); - for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int i = 0; i < val_3; ++i) { u[i] = tu[i] / theta; } vpMatrix u_skew = vpColVector::skew(u); @@ -214,14 +213,14 @@ void vpMatrix::computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const ctoInitSkew = ctoInitSkew * LthetauInvAnalytic; - for (unsigned int a = 0; a < 3; ++a) { - for (unsigned int b = 0; b < 3; ++b) { + for (unsigned int a = 0; a < val_3; ++a) { + for (unsigned int b = 0; b < val_3; ++b) { LpInv[a][b + 3] = ctoInitSkew[a][b]; } } - for (unsigned int a = 0; a < 3; ++a) { - for (unsigned int b = 0; b < 3; ++b) { + for (unsigned int a = 0; a < val_3; ++a) { + for (unsigned int b = 0; b < val_3; ++b) { LpInv[a + 3][b + 3] = LthetauInvAnalytic[a][b]; } } diff --git a/modules/core/src/math/matrix/vpMatrix_lu.cpp b/modules/core/src/math/matrix/vpMatrix_lu.cpp index 7730ee975e..b5d9b1564f 100644 --- a/modules/core/src/math/matrix/vpMatrix_lu.cpp +++ b/modules/core/src/math/matrix/vpMatrix_lu.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -94,32 +93,32 @@ BEGIN_VISP_NAMESPACE Here an example: \code -#include - -int main() -{ - vpMatrix A(4,4); + #include - A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.; - A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; A[1][3] = 1/5.; - A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/2.; A[2][3] = 1/6.; - A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.; - - // Compute the inverse - vpMatrix A_1 = A.inverseByLU(); - - std::cout << "Inverse by LU "; -#if defined(VISP_HAVE_LAPACK) - std::cout << "(using Lapack)"; -#elif defined(VISP_HAVE_EIGEN3) - std::cout << "(using Eigen3)"; -#elif defined(VISP_HAVE_OPENCV) - std::cout << "(using OpenCV)"; -#endif - std::cout << ": \n" << A_1 << std::endl; - - std::cout << "A*A^-1: \n" << A * A_1 << std::endl; -} + int main() + { + vpMatrix A(4,4); + + A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.; + A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; A[1][3] = 1/5.; + A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/2.; A[2][3] = 1/6.; + A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.; + + // Compute the inverse + vpMatrix A_1 = A.inverseByLU(); + + std::cout << "Inverse by LU "; + #if defined(VISP_HAVE_LAPACK) + std::cout << "(using Lapack)"; + #elif defined(VISP_HAVE_EIGEN3) + std::cout << "(using Eigen3)"; + #elif defined(VISP_HAVE_OPENCV) + std::cout << "(using OpenCV)"; + #endif + std::cout << ": \n" << A_1 << std::endl; + + std::cout << "A*A^-1: \n" << A * A_1 << std::endl; + } \endcode \sa inverseByLULapack(), inverseByLUEigen3(), inverseByLUOpenCV(), @@ -162,17 +161,20 @@ vpMatrix vpMatrix::inverseByLU() const rowNum, colNum)); } d = 1. / d; - inv[0][0] = (((*this)[1][1] * (*this)[2][2]) - ((*this)[1][2] * (*this)[2][1])) * d; - inv[0][1] = (((*this)[0][2] * (*this)[2][1]) - ((*this)[0][1] * (*this)[2][2])) * d; - inv[0][2] = (((*this)[0][1] * (*this)[1][2]) - ((*this)[0][2] * (*this)[1][1])) * d; - - inv[1][0] = (((*this)[1][2] * (*this)[2][0]) - ((*this)[1][0] * (*this)[2][2])) * d; - inv[1][1] = (((*this)[0][0] * (*this)[2][2]) - ((*this)[0][2] * (*this)[2][0])) * d; - inv[1][2] = (((*this)[0][2] * (*this)[1][0]) - ((*this)[0][0] * (*this)[1][2])) * d; - - inv[2][0] = (((*this)[1][0] * (*this)[2][1]) - ((*this)[1][1] * (*this)[2][0])) * d; - inv[2][1] = (((*this)[0][1] * (*this)[2][0]) - ((*this)[0][0] * (*this)[2][1])) * d; - inv[2][2] = (((*this)[0][0] * (*this)[1][1]) - ((*this)[0][1] * (*this)[1][0])) * d; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + inv[index_0][index_0] = (((*this)[index_1][index_1] * (*this)[index_2][index_2]) - ((*this)[index_1][index_2] * (*this)[index_2][index_1])) * d; + inv[index_0][index_1] = (((*this)[index_0][index_2] * (*this)[index_2][index_1]) - ((*this)[index_0][index_1] * (*this)[index_2][index_2])) * d; + inv[index_0][index_2] = (((*this)[index_0][index_1] * (*this)[index_1][index_2]) - ((*this)[index_0][index_2] * (*this)[index_1][index_1])) * d; + + inv[index_1][index_0] = (((*this)[index_1][index_2] * (*this)[index_2][index_0]) - ((*this)[index_1][index_0] * (*this)[index_2][index_2])) * d; + inv[index_1][index_1] = (((*this)[index_0][index_0] * (*this)[index_2][index_2]) - ((*this)[index_0][index_2] * (*this)[index_2][index_0])) * d; + inv[index_1][index_2] = (((*this)[index_0][index_2] * (*this)[index_1][index_0]) - ((*this)[index_0][index_0] * (*this)[index_1][index_2])) * d; + + inv[index_2][index_0] = (((*this)[index_1][index_0] * (*this)[index_2][index_1]) - ((*this)[index_1][index_1] * (*this)[index_2][index_0])) * d; + inv[index_2][index_1] = (((*this)[index_0][index_1] * (*this)[index_2][index_0]) - ((*this)[index_0][index_0] * (*this)[index_2][index_1])) * d; + inv[index_2][index_2] = (((*this)[index_0][index_0] * (*this)[index_1][index_1]) - ((*this)[index_0][index_1] * (*this)[index_1][index_0])) * d; return inv; } else { @@ -203,22 +205,22 @@ vpMatrix vpMatrix::inverseByLU() const \return The determinant of the matrix if the matrix is square. \code -#include + #include -#include + #include -int main() -{ - vpMatrix A(3,3); - A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; - A[1][0] = 1/3.; A[1][1] = 1/4.; A[1][2] = 1/5.; - A[2][0] = 1/6.; A[2][1] = 1/7.; A[2][2] = 1/8.; - std::cout << "Initial matrix: \n" << A << std::endl; - - // Compute the determinant - std:: cout << "Determinant by default method : " << A.det() << std::endl; - std:: cout << "Determinant by LU decomposition : " << A.detByLU() << std::endl; -} + int main() + { + vpMatrix A(3,3); + A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; + A[1][0] = 1/3.; A[1][1] = 1/4.; A[1][2] = 1/5.; + A[2][0] = 1/6.; A[2][1] = 1/7.; A[2][2] = 1/8.; + std::cout << "Initial matrix: \n" << A << std::endl; + + // Compute the determinant + std:: cout << "Determinant by default method : " << A.det() << std::endl; + std:: cout << "Determinant by LU decomposition : " << A.detByLU() << std::endl; + } \endcode \sa detByLULapack(), detByLUEigen3(), detByLUOpenCV() */ @@ -231,9 +233,12 @@ double vpMatrix::detByLU() const return (((*this)[0][0] * (*this)[1][1]) - ((*this)[0][1] * (*this)[1][0])); } else if ((rowNum == 3) && (colNum == 3)) { - return ((((*this)[0][0] * (((*this)[1][1] * (*this)[2][2]) - ((*this)[1][2] * (*this)[2][1]))) - - ((*this)[0][1] * (((*this)[1][0] * (*this)[2][2]) - ((*this)[1][2] * (*this)[2][0])))) + - ((*this)[0][2] * (((*this)[1][0] * (*this)[2][1]) - ((*this)[1][1] * (*this)[2][0])))); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + return ((((*this)[index_0][index_0] * (((*this)[index_1][index_1] * (*this)[index_2][index_2]) - ((*this)[index_1][index_2] * (*this)[index_2][index_1]))) - + ((*this)[index_0][index_1] * (((*this)[index_1][index_0] * (*this)[index_2][index_2]) - ((*this)[index_1][index_2] * (*this)[index_2][index_0])))) + + ((*this)[index_0][index_2] * (((*this)[index_1][index_0] * (*this)[index_2][index_1]) - ((*this)[index_1][index_1] * (*this)[index_2][index_0])))); } else { #if defined(VISP_HAVE_LAPACK) @@ -258,24 +263,24 @@ double vpMatrix::detByLU() const Here an example: \code -#include + #include -int main() -{ - vpMatrix A(4,4); + int main() + { + vpMatrix A(4,4); - A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.; - A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; A[1][3] = 1/5.; - A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/2.; A[2][3] = 1/6.; - A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.; + A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.; + A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; A[1][3] = 1/5.; + A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/2.; A[2][3] = 1/6.; + A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.; - // Compute the inverse - vpMatrix A_1; // A^-1 - A_1 = A.inverseByLULapack(); - std::cout << "Inverse by LU (Lapack): \n" << A_1 << std::endl; + // Compute the inverse + vpMatrix A_1; // A^-1 + A_1 = A.inverseByLULapack(); + std::cout << "Inverse by LU (Lapack): \n" << A_1 << std::endl; - std::cout << "A*A^-1: \n" << A * A_1 << std::endl; -} + std::cout << "A*A^-1: \n" << A * A_1 << std::endl; + } \endcode \sa inverseByLU(), inverseByLUEigen3(), inverseByLUOpenCV() @@ -359,21 +364,21 @@ vpMatrix vpMatrix::inverseByLULapack() const \return The determinant of the matrix if the matrix is square. \code -#include + #include -#include + #include -int main() -{ - vpMatrix A(3,3); - A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; - A[1][0] = 1/3.; A[1][1] = 1/4.; A[1][2] = 1/5.; - A[2][0] = 1/6.; A[2][1] = 1/7.; A[2][2] = 1/8.; - std::cout << "Initial matrix: \n" << A << std::endl; - - // Compute the determinant - std:: cout << "Determinant by LU decomposition (Lapack): " << A.detByLULapack() << std::endl; -} + int main() + { + vpMatrix A(3,3); + A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; + A[1][0] = 1/3.; A[1][1] = 1/4.; A[1][2] = 1/5.; + A[2][0] = 1/6.; A[2][1] = 1/7.; A[2][2] = 1/8.; + std::cout << "Initial matrix: \n" << A << std::endl; + + // Compute the determinant + std:: cout << "Determinant by LU decomposition (Lapack): " << A.detByLULapack() << std::endl; + } \endcode \sa detByLU(), detByLUEigen3(), detByLUOpenCV() */ @@ -454,30 +459,30 @@ double vpMatrix::detByLULapack() const #if defined(VISP_HAVE_OPENCV) /*! Compute the inverse of a n-by-n matrix using the LU decomposition with -OpenCV 3rd party. + OpenCV 3rd party. \return The inverse matrix. Here an example: \code -#include + #include -int main() -{ - vpMatrix A(4,4); + int main() + { + vpMatrix A(4,4); - A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.; - A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; A[1][3] = 1/5.; - A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/2.; A[2][3] = 1/6.; - A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.; + A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.; + A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; A[1][3] = 1/5.; + A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/2.; A[2][3] = 1/6.; + A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.; - // Compute the inverse - vpMatrix A_1; // A^-1 - A_1 = A.inverseByLUOpenCV(); - std::cout << "Inverse by LU (OpenCV): \n" << A_1 << std::endl; + // Compute the inverse + vpMatrix A_1; // A^-1 + A_1 = A.inverseByLUOpenCV(); + std::cout << "Inverse by LU (OpenCV): \n" << A_1 << std::endl; - std::cout << "A*A^-1: \n" << A * A_1 << std::endl; -} + std::cout << "A*A^-1: \n" << A * A_1 << std::endl; + } \endcode \sa inverseByLU(), inverseByLUEigen3(), inverseByLULapack() @@ -500,26 +505,26 @@ vpMatrix vpMatrix::inverseByLUOpenCV() const /*! Compute the determinant of a n-by-n matrix using the LU decomposition with -OpenCV 3rd party. + OpenCV 3rd party. \return Determinant of the matrix. \code -#include + #include -#include + #include -int main() -{ - vpMatrix A(3,3); - A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; - A[1][0] = 1/3.; A[1][1] = 1/4.; A[1][2] = 1/5.; - A[2][0] = 1/6.; A[2][1] = 1/7.; A[2][2] = 1/8.; - std::cout << "Initial matrix: \n" << A << std::endl; - - // Compute the determinant - std:: cout << "Determinant by LU decomposition (OpenCV): " << A.detByLUOpenCV() << std::endl; -} + int main() + { + vpMatrix A(3,3); + A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; + A[1][0] = 1/3.; A[1][1] = 1/4.; A[1][2] = 1/5.; + A[2][0] = 1/6.; A[2][1] = 1/7.; A[2][2] = 1/8.; + std::cout << "Initial matrix: \n" << A << std::endl; + + // Compute the determinant + std:: cout << "Determinant by LU decomposition (OpenCV): " << A.detByLUOpenCV() << std::endl; + } \endcode \sa detByLU(), detByLUEigen3(), detByLULapack() */ @@ -543,30 +548,30 @@ double vpMatrix::detByLUOpenCV() const /*! Compute the inverse of a n-by-n matrix using the LU decomposition with -Eigen3 3rd party. + Eigen3 3rd party. \return The inverse matrix. Here an example: \code -#include + #include -int main() -{ - vpMatrix A(4,4); + int main() + { + vpMatrix A(4,4); - A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.; - A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; A[1][3] = 1/5.; - A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/2.; A[2][3] = 1/6.; - A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.; + A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.; + A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; A[1][3] = 1/5.; + A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/2.; A[2][3] = 1/6.; + A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.; - // Compute the inverse - vpMatrix A_1; // A^-1 - A_1 = A.inverseByLUEigen3(); - std::cout << "Inverse by LU (Eigen3): \n" << A_1 << std::endl; + // Compute the inverse + vpMatrix A_1; // A^-1 + A_1 = A.inverseByLUEigen3(); + std::cout << "Inverse by LU (Eigen3): \n" << A_1 << std::endl; - std::cout << "A*A^-1: \n" << A * A_1 << std::endl; -} + std::cout << "A*A^-1: \n" << A * A_1 << std::endl; + } \endcode \sa inverseByLU(), inverseByLULapack(), inverseByLUOpenCV() @@ -590,26 +595,26 @@ vpMatrix vpMatrix::inverseByLUEigen3() const /*! Compute the determinant of a square matrix using the LU decomposition with -Eigen3 3rd party. + Eigen3 3rd party. \return The determinant of the matrix if the matrix is square. \code -#include + #include -#include + #include -int main() -{ - vpMatrix A(3,3); - A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; - A[1][0] = 1/3.; A[1][1] = 1/4.; A[1][2] = 1/5.; - A[2][0] = 1/6.; A[2][1] = 1/7.; A[2][2] = 1/8.; - std::cout << "Initial matrix: \n" << A << std::endl; - - // Compute the determinant - std:: cout << "Determinant by LU decomposition (Eigen3): " << A.detByLUEigen3() << std::endl; -} + int main() + { + vpMatrix A(3,3); + A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; + A[1][0] = 1/3.; A[1][1] = 1/4.; A[1][2] = 1/5.; + A[2][0] = 1/6.; A[2][1] = 1/7.; A[2][2] = 1/8.; + std::cout << "Initial matrix: \n" << A << std::endl; + + // Compute the determinant + std:: cout << "Determinant by LU decomposition (Eigen3): " << A.detByLUEigen3() << std::endl; + } \endcode \sa detByLU(), detByLUOpenCV(), detByLULapack() */ diff --git a/modules/core/src/math/matrix/vpMatrix_mul.cpp b/modules/core/src/math/matrix/vpMatrix_mul.cpp index 69aae24f45..64c42b397f 100644 --- a/modules/core/src/math/matrix/vpMatrix_mul.cpp +++ b/modules/core/src/math/matrix/vpMatrix_mul.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/core/src/math/matrix/vpMatrix_qr.cpp b/modules/core/src/math/matrix/vpMatrix_qr.cpp index 53b60a202c..ae28d008d2 100644 --- a/modules/core/src/math/matrix/vpMatrix_qr.cpp +++ b/modules/core/src/math/matrix/vpMatrix_qr.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,7 @@ * * Description: * Matrix QR decomposition. - * - * Authors: - * Filip Novotny - * -*****************************************************************************/ + */ #include // for (std::min) and (std::max) #include // For std::abs() on iOS @@ -49,9 +44,6 @@ #include #include -// Debug trace -#include - BEGIN_VISP_NAMESPACE #ifdef VISP_HAVE_LAPACK #ifdef VISP_HAVE_GSL @@ -128,23 +120,23 @@ void display_gsl(gsl_matrix *M) Here an example: \code -#include + #include -int main() -{ - vpMatrix A(4,4); + int main() + { + vpMatrix A(4,4); - A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.; - A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; A[1][3] = 1/5.; - A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/2.; A[2][3] = 1/6.; - A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.; + A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.; + A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; A[1][3] = 1/5.; + A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/2.; A[2][3] = 1/6.; + A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.; - // Compute the inverse - vpMatrix A_1 = A.inverseByQRLapack(); - std::cout << "Inverse by QR: \n" << A_1 << std::endl; + // Compute the inverse + vpMatrix A_1 = A.inverseByQRLapack(); + std::cout << "Inverse by QR: \n" << A_1 << std::endl; - std::cout << "A*A^-1: \n" << A * A_1 << std::endl; -} + std::cout << "A*A^-1: \n" << A * A_1 << std::endl; + } \endcode \sa inverseByQR() @@ -358,23 +350,23 @@ vpMatrix vpMatrix::inverseByQRLapack() const Here an example: \code -#include + #include -int main() -{ - vpMatrix A(4,4); + int main() + { + vpMatrix A(4,4); - A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.; - A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; A[1][3] = 1/5.; - A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/2.; A[2][3] = 1/6.; - A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.; + A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.; + A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; A[1][3] = 1/5.; + A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/2.; A[2][3] = 1/6.; + A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.; - // Compute the inverse - vpMatrix A_1 = A.inverseByQR(); - std::cout << "Inverse by QR: \n" << A_1 << std::endl; + // Compute the inverse + vpMatrix A_1 = A.inverseByQR(); + std::cout << "Inverse by QR: \n" << A_1 << std::endl; - std::cout << "A*A^-1: \n" << A * A_1 << std::endl; -} + std::cout << "A*A^-1: \n" << A * A_1 << std::endl; + } \endcode \sa inverseByLU(), inverseByCholesky() @@ -411,33 +403,33 @@ vpMatrix vpMatrix::inverseByQR() const Here an example: \code -#include + #include -double residual(vpMatrix M1, vpMatrix M2) -{ - return (M1 - M2).frobeniusNorm(); -} + double residual(vpMatrix M1, vpMatrix M2) + { + return (M1 - M2).frobeniusNorm(); + } -int main() -{ - vpMatrix A(4,3); - - A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; - A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; - A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/2.; - A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/6.; - - // Economic QR (Q 4x3, R 3x3) - vpMatrix Q, R; - int r = A.qr(A, R); - std::cout << "QR Residual: " - << residual(A, Q*R) << std::endl; - - // Full QR (Q 4x4, R 3x3) - r = A.qr(Q, R, true); - std::cout << "Full QR Residual: " - << residual(A, Q.extract(0, 0, 4, 3)*R) << std::endl; -} + int main() + { + vpMatrix A(4,3); + + A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; + A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; + A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/2.; + A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/6.; + + // Economic QR (Q 4x3, R 3x3) + vpMatrix Q, R; + int r = A.qr(A, R); + std::cout << "QR Residual: " + << residual(A, Q*R) << std::endl; + + // Full QR (Q 4x4, R 3x3) + r = A.qr(Q, R, true); + std::cout << "Full QR Residual: " + << residual(A, Q.extract(0, 0, 4, 3)*R) << std::endl; + } \endcode \sa qrPivot() @@ -686,40 +678,40 @@ unsigned int vpMatrix::qr(vpMatrix &Q, vpMatrix &R, bool full, bool squareR, dou Here an example: \code -#include - -double residual(vpMatrix M1, vpMatrix M2) -{ - return (M1 - M2).frobeniusNorm(); -} + #include -int main() -{ - vpMatrix A(4,3); - - A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/2.; - A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; - A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/4.; - A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/5.; - // A is (4x3) but rank 2 + double residual(vpMatrix M1, vpMatrix M2) + { + return (M1 - M2).frobeniusNorm(); + } - // Economic QR (Q 4x3, R 3x3) - vpMatrix Q, R, P; - int r = A.qrPivot(Q, R, P); - std::cout << "A rank: " << r << std::endl; - std::cout << "Residual: " << residual(A*P, Q*R) << std::endl; - - // Full QR (Q 4x4, R 3x3) - r = A.qrPivot(Q, R, P, true); - std::cout << "QRPivot Residual: " << - residual(A*P, Q.extract(0, 0, 4, 3)*R) << std::endl; - - // Using permutation matrix: keep only non-null part of R - Q.resize(4, r, false); // Q is 4 x 2 - R = R.extract(0, 0, r, 3)*P.t(); // R is 2 x 3 - std::cout << "Full QRPivot Residual: " << - residual(A, Q*R) << std::endl; -} + int main() + { + vpMatrix A(4,3); + + A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/2.; + A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; + A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/4.; + A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/5.; + // A is (4x3) but rank 2 + + // Economic QR (Q 4x3, R 3x3) + vpMatrix Q, R, P; + int r = A.qrPivot(Q, R, P); + std::cout << "A rank: " << r << std::endl; + std::cout << "Residual: " << residual(A*P, Q*R) << std::endl; + + // Full QR (Q 4x4, R 3x3) + r = A.qrPivot(Q, R, P, true); + std::cout << "QRPivot Residual: " << + residual(A*P, Q.extract(0, 0, 4, 3)*R) << std::endl; + + // Using permutation matrix: keep only non-null part of R + Q.resize(4, r, false); // Q is 4 x 2 + R = R.extract(0, 0, r, 3)*P.t(); // R is 2 x 3 + std::cout << "Full QRPivot Residual: " << + residual(A, Q*R) << std::endl; + } \endcode \sa qrPivot() @@ -1124,31 +1116,31 @@ vpMatrix vpMatrix::inverseTriangular(bool upper) const Here an example: \code - #include - #include - int main() - { - vpMatrix A(3,3); - A[0][0] = 4.64; - A[0][1] = 0.288; - A[0][2] = -0.384; - A[1][0] = 0.288; - A[1][1] = 7.3296; - A[1][2] = 2.2272; - A[2][0] = -0.384; - A[2][1] = 2.2272; - A[2][2] = 6.0304; - vpColVector X(3), B(3); - B[0] = 1; - B[1] = 2; - B[2] = 3; - A.solveByQR(B, X); - // Obtained values of X - // X[0] = 0.2468; - // X[1] = 0.120782; - // X[2] = 0.468587; - std::cout << "X:\n" << X << std::endl; - } + #include + #include + int main() + { + vpMatrix A(3,3); + A[0][0] = 4.64; + A[0][1] = 0.288; + A[0][2] = -0.384; + A[1][0] = 0.288; + A[1][1] = 7.3296; + A[1][2] = 2.2272; + A[2][0] = -0.384; + A[2][1] = 2.2272; + A[2][2] = 6.0304; + vpColVector X(3), B(3); + B[0] = 1; + B[1] = 2; + B[2] = 3; + A.solveByQR(B, X); + // Obtained values of X + // X[0] = 0.2468; + // X[1] = 0.120782; + // X[2] = 0.468587; + std::cout << "X:\n" << X << std::endl; + } \endcode \sa qrPivot() @@ -1174,31 +1166,31 @@ void vpMatrix::solveByQR(const vpColVector &b, vpColVector &x) const Here an example: \code - #include - #include - int main() - { - vpMatrix A(3,3); - A[0][0] = 4.64; - A[0][1] = 0.288; - A[0][2] = -0.384; - A[1][0] = 0.288; - A[1][1] = 7.3296; - A[1][2] = 2.2272; - A[2][0] = -0.384; - A[2][1] = 2.2272; - A[2][2] = 6.0304; - vpColVector X(3), B(3); - B[0] = 1; - B[1] = 2; - B[2] = 3; - X = A.solveByQR(B); - // Obtained values of X - // X[0] = 0.2468; - // X[1] = 0.120782; - // X[2] = 0.468587; - std::cout << "X:\n" << X << std::endl; - } + #include + #include + int main() + { + vpMatrix A(3,3); + A[0][0] = 4.64; + A[0][1] = 0.288; + A[0][2] = -0.384; + A[1][0] = 0.288; + A[1][1] = 7.3296; + A[1][2] = 2.2272; + A[2][0] = -0.384; + A[2][1] = 2.2272; + A[2][2] = 6.0304; + vpColVector X(3), B(3); + B[0] = 1; + B[1] = 2; + B[2] = 3; + X = A.solveByQR(B); + // Obtained values of X + // X[0] = 0.2468; + // X[1] = 0.120782; + // X[2] = 0.468587; + std::cout << "X:\n" << X << std::endl; + } \endcode \sa qrPivot() diff --git a/modules/core/src/math/matrix/vpMatrix_svd.cpp b/modules/core/src/math/matrix/vpMatrix_svd.cpp index 70c9121778..146d0dd8c8 100644 --- a/modules/core/src/math/matrix/vpMatrix_svd.cpp +++ b/modules/core/src/math/matrix/vpMatrix_svd.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -35,7 +34,6 @@ #include #include -#include #include #include #include @@ -394,45 +392,45 @@ void vpMatrix::svdLapack(vpColVector &w, vpMatrix &V) Here an example of SVD decomposition of a non square Matrix M. -\code -#include -#include + \code + #include + #include -int main() -{ - vpMatrix M(3,2); - M[0][0] = 1; - M[1][0] = 2; - M[2][0] = 0.5; - - M[0][1] = 6; - M[1][1] = 8 ; - M[2][1] = 9 ; - - vpMatrix V; - vpColVector w; - vpMatrix Mrec; - vpMatrix Sigma; - - M.svdEigen3(w, V); - // Here M is modified and is now equal to U - - // Construct the diagonal matrix from the singular values - Sigma.diag(w); - - // Reconstruct the initial matrix M using the decomposition - Mrec = M * Sigma * V.t(); - - // Here, Mrec is obtained equal to the initial value of M - // Mrec[0][0] = 1; - // Mrec[1][0] = 2; - // Mrec[2][0] = 0.5; - // Mrec[0][1] = 6; - // Mrec[1][1] = 8 ; - // Mrec[2][1] = 9 ; - - std::cout << "Reconstructed M matrix: \n" << Mrec << std::endl; -} + int main() + { + vpMatrix M(3,2); + M[0][0] = 1; + M[1][0] = 2; + M[2][0] = 0.5; + + M[0][1] = 6; + M[1][1] = 8 ; + M[2][1] = 9 ; + + vpMatrix V; + vpColVector w; + vpMatrix Mrec; + vpMatrix Sigma; + + M.svdEigen3(w, V); + // Here M is modified and is now equal to U + + // Construct the diagonal matrix from the singular values + Sigma.diag(w); + + // Reconstruct the initial matrix M using the decomposition + Mrec = M * Sigma * V.t(); + + // Here, Mrec is obtained equal to the initial value of M + // Mrec[0][0] = 1; + // Mrec[1][0] = 2; + // Mrec[2][0] = 0.5; + // Mrec[0][1] = 6; + // Mrec[1][1] = 8 ; + // Mrec[2][1] = 9 ; + + std::cout << "Reconstructed M matrix: \n" << Mrec << std::endl; + } \endcode \sa svd(), svdLapack(), svdOpenCV() diff --git a/modules/core/src/math/matrix/vpRowVector.cpp b/modules/core/src/math/matrix/vpRowVector.cpp index 3ea3b992dd..b4573e870c 100644 --- a/modules/core/src/math/matrix/vpRowVector.cpp +++ b/modules/core/src/math/matrix/vpRowVector.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -46,7 +45,6 @@ #include #include -#include #include #include #include @@ -1153,27 +1151,27 @@ double vpRowVector::euclideanNorm() const { return frobeniusNorm(); } is used to initialize the constructed row vector. The following code shows how to use this function: -\code -#include + \code + #include + + int main() + { + vpRowVector v(4); + int val = 0; + for(size_t i=0; i + \code + #include -int main() -{ - vpRowVector r(3); - for (unsigned int i=0; i diff --git a/modules/core/src/math/matrix/vpSubMatrix.cpp b/modules/core/src/math/matrix/vpSubMatrix.cpp index c906c316f7..1cf6cb92d4 100644 --- a/modules/core/src/math/matrix/vpSubMatrix.cpp +++ b/modules/core/src/math/matrix/vpSubMatrix.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,7 @@ * * Description: * Mask on a vpMatrix . - * - * Authors: - * Laneurit Jean - * -*****************************************************************************/ + */ #include #include diff --git a/modules/core/src/math/matrix/vpSubRowVector.cpp b/modules/core/src/math/matrix/vpSubRowVector.cpp index 875b02cec4..b9b3088354 100644 --- a/modules/core/src/math/matrix/vpSubRowVector.cpp +++ b/modules/core/src/math/matrix/vpSubRowVector.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/core/src/math/misc/vpMath.cpp b/modules/core/src/math/misc/vpMath.cpp index cba531ab04..a9a3480042 100644 --- a/modules/core/src/math/misc/vpMath.cpp +++ b/modules/core/src/math/misc/vpMath.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -489,20 +488,23 @@ vpHomogeneousMatrix vpMath::ned2ecef(double lonDeg, double latDeg, double radius { double lon = vpMath::rad(lonDeg); double lat = vpMath::rad(latDeg); - + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; vpHomogeneousMatrix ecef_M_ned; - ecef_M_ned[0][0] = -sin(lat) * cos(lon); - ecef_M_ned[0][1] = -sin(lon); - ecef_M_ned[0][2] = -cos(lat) * cos(lon); - ecef_M_ned[0][3] = radius * cos(lon) * cos(lat); - ecef_M_ned[1][0] = -sin(lat) * sin(lon); - ecef_M_ned[1][1] = cos(lon); - ecef_M_ned[1][2] = -cos(lat) * sin(lon); - ecef_M_ned[1][3] = radius * sin(lon) * cos(lat); - ecef_M_ned[2][0] = cos(lat); - ecef_M_ned[2][1] = 0; - ecef_M_ned[2][2] = -sin(lat); - ecef_M_ned[2][3] = radius * sin(lat); + ecef_M_ned[index_0][index_0] = -sin(lat) * cos(lon); + ecef_M_ned[index_0][index_1] = -sin(lon); + ecef_M_ned[index_0][index_2] = -cos(lat) * cos(lon); + ecef_M_ned[index_0][index_3] = radius * cos(lon) * cos(lat); + ecef_M_ned[index_1][index_0] = -sin(lat) * sin(lon); + ecef_M_ned[index_1][index_1] = cos(lon); + ecef_M_ned[index_1][index_2] = -cos(lat) * sin(lon); + ecef_M_ned[index_1][index_3] = radius * sin(lon) * cos(lat); + ecef_M_ned[index_2][index_0] = cos(lat); + ecef_M_ned[index_2][index_1] = 0; + ecef_M_ned[index_2][index_2] = -sin(lat); + ecef_M_ned[index_2][index_3] = radius * sin(lat); return ecef_M_ned; } @@ -550,20 +552,24 @@ vpHomogeneousMatrix vpMath::enu2ecef(double lonDeg, double latDeg, double radius { double lon = vpMath::rad(lonDeg); double lat = vpMath::rad(latDeg); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; vpHomogeneousMatrix ecef_M_enu; - ecef_M_enu[0][0] = -sin(lon); - ecef_M_enu[0][1] = -sin(lat) * cos(lon); - ecef_M_enu[0][2] = cos(lat) * cos(lon); - ecef_M_enu[0][3] = radius * cos(lon) * cos(lat); - ecef_M_enu[1][0] = cos(lon); - ecef_M_enu[1][1] = -sin(lat) * sin(lon); - ecef_M_enu[1][2] = cos(lat) * sin(lon); - ecef_M_enu[1][3] = radius * sin(lon) * cos(lat); - ecef_M_enu[2][0] = 0; - ecef_M_enu[2][1] = cos(lat); - ecef_M_enu[2][2] = sin(lat); - ecef_M_enu[2][3] = radius * sin(lat); + ecef_M_enu[index_0][index_0] = -sin(lon); + ecef_M_enu[index_0][index_1] = -sin(lat) * cos(lon); + ecef_M_enu[index_0][index_2] = cos(lat) * cos(lon); + ecef_M_enu[index_0][index_3] = radius * cos(lon) * cos(lat); + ecef_M_enu[index_1][index_0] = cos(lon); + ecef_M_enu[index_1][index_1] = -sin(lat) * sin(lon); + ecef_M_enu[index_1][index_2] = cos(lat) * sin(lon); + ecef_M_enu[index_1][index_3] = radius * sin(lon) * cos(lat); + ecef_M_enu[index_2][index_0] = 0; + ecef_M_enu[index_2][index_1] = cos(lat); + ecef_M_enu[index_2][index_2] = sin(lat); + ecef_M_enu[index_2][index_3] = radius * sin(lat); return ecef_M_enu; } @@ -677,20 +683,24 @@ vpHomogeneousMatrix vpMath::lookAt(const vpColVector &from, const vpColVector &t vpColVector forward = (from - to).normalize(); vpColVector right = vpColVector::crossProd(tmp.normalize(), forward).normalize(); vpColVector up = vpColVector::crossProd(forward, right).normalize(); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; vpHomogeneousMatrix wMc; - wMc[0][0] = right[0]; - wMc[0][1] = up[0]; - wMc[0][2] = forward[0]; - wMc[0][3] = from[0]; - wMc[1][0] = right[1]; - wMc[1][1] = up[1]; - wMc[1][2] = forward[1]; - wMc[1][3] = from[1]; - wMc[2][0] = right[2]; - wMc[2][1] = up[2]; - wMc[2][2] = forward[2]; - wMc[2][3] = from[2]; + wMc[index_0][index_0] = right[index_0]; + wMc[index_0][index_1] = up[index_0]; + wMc[index_0][index_2] = forward[index_0]; + wMc[index_0][index_3] = from[index_0]; + wMc[index_1][index_0] = right[index_1]; + wMc[index_1][index_1] = up[index_1]; + wMc[index_1][index_2] = forward[index_1]; + wMc[index_1][index_3] = from[index_1]; + wMc[index_2][index_0] = right[index_2]; + wMc[index_2][index_1] = up[index_2]; + wMc[index_2][index_2] = forward[index_2]; + wMc[index_2][index_3] = from[index_2]; return wMc; } @@ -754,11 +764,14 @@ vpColVector vpMath::rad(const vpColVector &r) vpHomogeneousMatrix vpMath::enu2ned(const vpHomogeneousMatrix &enu_M) { vpHomogeneousMatrix ned_M_enu; - ned_M_enu[0][0] = 0; - ned_M_enu[0][1] = 1; - ned_M_enu[1][0] = 1; - ned_M_enu[1][1] = 0; - ned_M_enu[2][2] = -1; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + ned_M_enu[index_0][index_0] = 0; + ned_M_enu[index_0][index_1] = 1; + ned_M_enu[index_1][index_0] = 1; + ned_M_enu[index_1][index_1] = 0; + ned_M_enu[index_2][index_2] = -1; vpHomogeneousMatrix ned_M = ned_M_enu * enu_M; return ned_M; diff --git a/modules/core/src/math/random-generator/vpUniRand.cpp b/modules/core/src/math/random-generator/vpUniRand.cpp index bc091ae5e4..79157f99e8 100644 --- a/modules/core/src/math/random-generator/vpUniRand.cpp +++ b/modules/core/src/math/random-generator/vpUniRand.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/core/src/math/robust/vpRobust.cpp b/modules/core/src/math/robust/vpRobust.cpp index c420c103cc..ab2005ca3f 100644 --- a/modules/core/src/math/robust/vpRobust.cpp +++ b/modules/core/src/math/robust/vpRobust.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,12 +29,7 @@ * * Description: * M-Estimator and various influence function. - * - * Authors: - * Andrew Comport - * Jean Laneurit - * -*****************************************************************************/ + */ /*! \file vpRobust.cpp @@ -49,9 +43,9 @@ #include #include -#include #include #include +#include BEGIN_VISP_NAMESPACE /*! @@ -272,12 +266,13 @@ int vpRobust::partition(vpColVector &a, int l, int r) double v = a[r]; for (;;) { - while (a[++i] < v) - ; - while (v < a[--j]) + while (a[++i] < v) { } + + while (v < a[--j]) { if (j == l) { break; } + } if (i >= j) { break; } diff --git a/modules/core/src/math/transformation/vpExponentialMap.cpp b/modules/core/src/math/transformation/vpExponentialMap.cpp index 2bbe6611fd..f99c443249 100644 --- a/modules/core/src/math/transformation/vpExponentialMap.cpp +++ b/modules/core/src/math/transformation/vpExponentialMap.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,7 @@ * * Description: * Exponential map. - * - * Authors: - * Francois Chaumette - * -*****************************************************************************/ + */ #include @@ -78,7 +73,15 @@ vpHomogeneousMatrix vpExponentialMap::direct(const vpColVector &v) { return vpEx */ vpHomogeneousMatrix vpExponentialMap::direct(const vpColVector &v, const double &delta_t) { - if (v.size() != 6) { + const unsigned int v_size = 6; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + const unsigned int index_5 = 5; + + if (v.size() != v_size) { throw(vpException(vpException::dimensionError, "Cannot compute direct exponential map from a %d-dim velocity vector. Should be 6-dim.", v.size())); @@ -90,103 +93,34 @@ vpHomogeneousMatrix vpExponentialMap::direct(const vpColVector &v, const double vpColVector v_dt = v * delta_t; - u[0] = v_dt[3]; - u[1] = v_dt[4]; - u[2] = v_dt[5]; + u[index_0] = v_dt[index_3]; + u[index_1] = v_dt[index_4]; + u[index_2] = v_dt[index_5]; rd.build(u); - theta = sqrt((u[0] * u[0]) + (u[1] * u[1]) + (u[2] * u[2])); + theta = sqrt((u[index_0] * u[index_0]) + (u[index_1] * u[index_1]) + (u[index_2] * u[index_2])); si = sin(theta); co = cos(theta); sinc = vpMath::sinc(si, theta); mcosc = vpMath::mcosc(co, theta); msinc = vpMath::msinc(si, theta); - dt[0] = ((v_dt[0] * (sinc + (u[0] * u[0] * msinc))) + - (v_dt[1] * ((u[0] * u[1] * msinc) - (u[2] * mcosc)))) + - (v_dt[2] * ((u[0] * u[2] * msinc) + (u[1] * mcosc))); + dt[index_0] = ((v_dt[index_0] * (sinc + (u[index_0] * u[index_0] * msinc))) + + (v_dt[index_1] * ((u[index_0] * u[index_1] * msinc) - (u[index_2] * mcosc)))) + + (v_dt[index_2] * ((u[index_0] * u[index_2] * msinc) + (u[index_1] * mcosc))); - dt[1] = ((v_dt[0] * ((u[0] * u[1] * msinc) + (u[2] * mcosc))) + - (v_dt[1] * (sinc + (u[1] * u[1] * msinc)))) + - (v_dt[2] * ((u[1] * u[2] * msinc) - (u[0] * mcosc))); + dt[index_1] = ((v_dt[index_0] * ((u[index_0] * u[index_1] * msinc) + (u[index_2] * mcosc))) + + (v_dt[index_1] * (sinc + (u[index_1] * u[index_1] * msinc)))) + + (v_dt[index_2] * ((u[index_1] * u[index_2] * msinc) - (u[index_0] * mcosc))); - dt[2] = ((v_dt[0] * ((u[0] * u[2] * msinc) - (u[1] * mcosc))) + - (v_dt[1] * ((u[1] * u[2] * msinc) + (u[0] * mcosc)))) + - (v_dt[2] * (sinc + (u[2] * u[2] * msinc))); + dt[index_2] = ((v_dt[index_0] * ((u[index_0] * u[index_2] * msinc) - (u[index_1] * mcosc))) + + (v_dt[index_1] * ((u[index_1] * u[index_2] * msinc) + (u[index_0] * mcosc)))) + + (v_dt[index_2] * (sinc + (u[index_2] * u[index_2] * msinc))); vpHomogeneousMatrix Delta; Delta.insert(rd); Delta.insert(dt); - if (0) // test new version wrt old version - { - // old version - unsigned int i, j; - - double s; - - s = sqrt((v_dt[3] * v_dt[3]) + (v_dt[4] * v_dt[4]) + (v_dt[5] * v_dt[5])); - if (s > 1.e-15) { - for (i = 0; i < 3; ++i) { - u[i] = v_dt[3 + i] / s; - } - double sinu = sin(s); - double cosi = cos(s); - double mcosi = 1 - cosi; - rd[0][0] = cosi + (mcosi * u[0] * u[0]); - rd[0][1] = (-sinu * u[2]) + (mcosi * u[0] * u[1]); - rd[0][2] = (sinu * u[1]) + (mcosi * u[0] * u[2]); - rd[1][0] = (sinu * u[2]) + (mcosi * u[1] * u[0]); - rd[1][1] = cosi + (mcosi * u[1] * u[1]); - rd[1][2] = (-sinu * u[0]) + (mcosi * u[1] * u[2]); - rd[2][0] = (-sinu * u[1]) + (mcosi * u[2] * u[0]); - rd[2][1] = (sinu * u[0]) + (mcosi * u[2] * u[1]); - rd[2][2] = cosi + (mcosi * u[2] * u[2]); - - dt[0] = (v_dt[0] * ((sinu / s) + (u[0] * u[0] * (1 - (sinu / s))))) + - (v_dt[1] * ((u[0] * u[1] * (1 - (sinu / s))) - ((u[2] * mcosi) / s))) + - (v_dt[2] * ((u[0] * u[2] * (1 - (sinu / s))) + ((u[1] * mcosi) / s))); - - dt[1] = (v_dt[0] * ((u[0] * u[1] * (1 - (sinu / s))) + ((u[2] * mcosi) / s))) + - (v_dt[1] * ((sinu / s) + (u[1] * u[1] * (1 - (sinu / s))))) + - (v_dt[2] * ((u[1] * u[2] * (1 - (sinu / s))) - ((u[0] * mcosi) / s))); - - dt[2] = (v_dt[0] * ((u[0] * u[2] * (1 - (sinu / s))) - ((u[1] * mcosi) / s))) + - (v_dt[1] * ((u[1] * u[2] * (1 - (sinu / s))) + ((u[0] * mcosi) / s))) + - (v_dt[2] * ((sinu / s) + (u[2] * u[2] * (1 - (sinu / s))))); - } - else { - for (i = 0; i < 3; ++i) { - for (j = 0; j < 3; ++j) { - rd[i][j] = 0.0; - } - rd[i][i] = 1.0; - dt[i] = v_dt[i]; - } - } - // end old version - - // Test of the new version - vpHomogeneousMatrix Delta_old; - Delta_old.insert(rd); - Delta_old.insert(dt); - - int pb = 0; - for (i = 0; i < 4; ++i) { - for (j = 0; j < 4; ++j) { - if (fabs(Delta[i][j] - Delta_old[i][j]) > 1.e-5) { - pb = 1; - } - } - } - if (pb == 1) { - printf("pb vpHomogeneousMatrix::expMap\n"); - std::cout << " Delta : " << std::endl << Delta << std::endl; - std::cout << " Delta_old : " << std::endl << Delta_old << std::endl; - } - // end of the test - } - return Delta; } @@ -231,6 +165,10 @@ vpColVector vpExponentialMap::inverse(const vpHomogeneousMatrix &M, const double vpThetaUVector u; vpRotationMatrix Rd, a; vpTranslationVector dt; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; M.extract(Rd); u.build(Rd); @@ -238,7 +176,7 @@ vpColVector vpExponentialMap::inverse(const vpHomogeneousMatrix &M, const double v[3 + i] = u[i]; } - theta = sqrt((u[0] * u[0]) + (u[1] * u[1]) + (u[2] * u[2])); + theta = sqrt((u[index_0] * u[index_0]) + (u[index_1] * u[index_1]) + (u[index_2] * u[index_2])); si = sin(theta); co = cos(theta); sinc = vpMath::sinc(si, theta); @@ -249,36 +187,36 @@ vpColVector vpExponentialMap::inverse(const vpHomogeneousMatrix &M, const double // the Rodrigues formula : sinc I + (1-sinc)/t^2 VV^T + (1-cos)/t^2 [V]_X // with V = t.U - a[0][0] = sinc + (u[0] * u[0] * msinc); - a[0][1] = (u[0] * u[1] * msinc) - (u[2] * mcosc); - a[0][2] = (u[0] * u[2] * msinc) + (u[1] * mcosc); + a[index_0][index_0] = sinc + (u[index_0] * u[index_0] * msinc); + a[index_0][index_1] = (u[index_0] * u[index_1] * msinc) - (u[index_2] * mcosc); + a[index_0][index_2] = (u[index_0] * u[index_2] * msinc) + (u[index_1] * mcosc); - a[1][0] = (u[0] * u[1] * msinc) + (u[2] * mcosc); - a[1][1] = sinc + (u[1] * u[1] * msinc); - a[1][2] = (u[1] * u[2] * msinc) - (u[0] * mcosc); + a[index_1][index_0] = (u[index_0] * u[index_1] * msinc) + (u[index_2] * mcosc); + a[index_1][index_1] = sinc + (u[index_1] * u[index_1] * msinc); + a[index_1][index_2] = (u[index_1] * u[index_2] * msinc) - (u[index_0] * mcosc); - a[2][0] = (u[0] * u[2] * msinc) - (u[1] * mcosc); - a[2][1] = (u[1] * u[2] * msinc) + (u[0] * mcosc); - a[2][2] = sinc + (u[2] * u[2] * msinc); + a[index_2][index_0] = (u[index_0] * u[index_2] * msinc) - (u[index_1] * mcosc); + a[index_2][index_1] = (u[index_1] * u[index_2] * msinc) + (u[index_0] * mcosc); + a[index_2][index_2] = sinc + (u[index_2] * u[index_2] * msinc); - det = (((((a[0][0] * a[1][1] * a[2][2]) + (a[1][0] * a[2][1] * a[0][2])) + (a[0][1] * a[1][2] * a[2][0])) - - (a[2][0] * a[1][1] * a[0][2])) - (a[1][0] * a[0][1] * a[2][2])) - (a[0][0] * a[2][1] * a[1][2]); + det = (((((a[index_0][index_0] * a[index_1][index_1] * a[index_2][index_2]) + (a[index_1][index_0] * a[index_2][index_1] * a[index_0][index_2])) + (a[index_0][index_1] * a[index_1][index_2] * a[index_2][index_0])) - + (a[index_2][index_0] * a[index_1][index_1] * a[index_0][index_2])) - (a[index_1][index_0] * a[index_0][index_1] * a[index_2][index_2])) - (a[index_0][index_0] * a[index_2][index_1] * a[index_1][index_2]); if (fabs(det) > 1.e-5) { - v[0] = ((((((M[0][3] * a[1][1] * a[2][2]) + (M[1][3] * a[2][1] * a[0][2])) + (M[2][3] * a[0][1] * a[1][2])) - - (M[2][3] * a[1][1] * a[0][2])) - (M[1][3] * a[0][1] * a[2][2])) - (M[0][3] * a[2][1] * a[1][2])) / + v[index_0] = ((((((M[index_0][index_3] * a[index_1][index_1] * a[index_2][index_2]) + (M[index_1][index_3] * a[index_2][index_1] * a[index_0][index_2])) + (M[index_2][index_3] * a[index_0][index_1] * a[index_1][index_2])) - + (M[index_2][index_3] * a[index_1][index_1] * a[index_0][index_2])) - (M[index_1][index_3] * a[index_0][index_1] * a[index_2][index_2])) - (M[index_0][index_3] * a[index_2][index_1] * a[index_1][index_2])) / det; - v[1] = ((((((a[0][0] * M[1][3] * a[2][2]) + (a[1][0] * M[2][3] * a[0][2])) + (M[0][3] * a[1][2] * a[2][0])) - - (a[2][0] * M[1][3] * a[0][2])) - (a[1][0] * M[0][3] * a[2][2])) - (a[0][0] * M[2][3] * a[1][2])) / + v[index_1] = ((((((a[index_0][index_0] * M[index_1][index_3] * a[index_2][index_2]) + (a[index_1][index_0] * M[index_2][index_3] * a[index_0][index_2])) + (M[index_0][index_3] * a[index_1][index_2] * a[index_2][index_0])) - + (a[index_2][index_0] * M[index_1][index_3] * a[index_0][index_2])) - (a[index_1][index_0] * M[index_0][index_3] * a[index_2][index_2])) - (a[index_0][index_0] * M[index_2][index_3] * a[index_1][index_2])) / det; - v[2] = ((((((a[0][0] * a[1][1] * M[2][3]) + (a[1][0] * a[2][1] * M[0][3])) + (a[0][1] * M[1][3] * a[2][0])) - - (a[2][0] * a[1][1] * M[0][3])) - (a[1][0] * a[0][1] * M[2][3])) - (a[0][0] * a[2][1] * M[1][3])) / + v[index_2] = ((((((a[index_0][index_0] * a[index_1][index_1] * M[index_2][index_3]) + (a[index_1][index_0] * a[index_2][index_1] * M[index_0][index_3])) + (a[index_0][index_1] * M[index_1][index_3] * a[index_2][index_0])) - + (a[index_2][index_0] * a[index_1][index_1] * M[index_0][index_3])) - (a[index_1][index_0] * a[index_0][index_1] * M[index_2][index_3])) - (a[index_0][index_0] * a[index_2][index_1] * M[index_1][index_3])) / det; } else { - v[0] = M[0][3]; - v[1] = M[1][3]; - v[2] = M[2][3]; + v[index_0] = M[index_0][index_3]; + v[index_1] = M[index_1][index_3]; + v[index_2] = M[index_2][index_3]; } // Apply the sampling time to the computed velocity diff --git a/modules/core/src/math/transformation/vpForceTwistMatrix.cpp b/modules/core/src/math/transformation/vpForceTwistMatrix.cpp index 5f4820af21..58e5dc8cb4 100644 --- a/modules/core/src/math/transformation/vpForceTwistMatrix.cpp +++ b/modules/core/src/math/transformation/vpForceTwistMatrix.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -45,7 +44,6 @@ #include #include -#include #include #include @@ -72,8 +70,9 @@ vpForceTwistMatrix &vpForceTwistMatrix::operator=(const vpForceTwistMatrix &M) */ void vpForceTwistMatrix::eye() { - for (unsigned int i = 0; i < 6; ++i) { - for (unsigned int j = 0; j < 6; ++j) { + const unsigned int nparam = 6; + for (unsigned int i = 0; i < nparam; ++i) { + for (unsigned int j = 0; j < nparam; ++j) { if (i == j) { (*this)[i][j] = 1.0; } @@ -274,11 +273,11 @@ int main() vpForceTwistMatrix vpForceTwistMatrix::operator*(const vpForceTwistMatrix &F) const { vpForceTwistMatrix Fout; - - for (unsigned int i = 0; i < 6; ++i) { - for (unsigned int j = 0; j < 6; ++j) { + const unsigned int nparam = 6; + for (unsigned int i = 0; i < nparam; ++i) { + for (unsigned int j = 0; j < nparam; ++j) { double s = 0; - for (unsigned int k = 0; k < 6; ++k) { + for (unsigned int k = 0; k < nparam; ++k) { s += rowPtrs[i][k] * F.rowPtrs[k][j]; } Fout[i][j] = s; @@ -296,18 +295,18 @@ vpForceTwistMatrix vpForceTwistMatrix::operator*(const vpForceTwistMatrix &F) co */ vpMatrix vpForceTwistMatrix::operator*(const vpMatrix &M) const { - - if (6 != M.getRows()) { + const unsigned int nparam = 6; + if (nparam != M.getRows()) { throw(vpException(vpException::dimensionError, "Cannot multiply (6x6) force/torque twist matrix by a (%dx%d) matrix", M.getRows(), M.getCols())); } unsigned int m_col = M.getCols(); - vpMatrix p(6, M.getCols()); - for (unsigned int i = 0; i < 6; ++i) { + vpMatrix p(nparam, M.getCols()); + for (unsigned int i = 0; i < nparam; ++i) { for (unsigned int j = 0; j < m_col; ++j) { double s = 0; - for (unsigned int k = 0; k < 6; ++k) { + for (unsigned int k = 0; k < nparam; ++k) { s += rowPtrs[i][k] * M[k][j]; } p[i][j] = s; @@ -319,41 +318,40 @@ vpMatrix vpForceTwistMatrix::operator*(const vpMatrix &M) const /*! Operator that allows to multiply a force/torque skew transformation matrix -by a column vector. + by a column vector. - \param H : Force/torque skew vector \f${\bf H} = [f_x, f_y, f_z, \tau_x, -\tau_y, \tau_z] \f$. + \param H : Force/torque skew vector \f${\bf H} = [f_x, f_y, f_z, \tau_x, \tau_y, \tau_z] \f$. For example, this operator can be used to convert a force/torque skew from -sensor frame into the probe frame : + sensor frame into the probe frame : \f[{^p}{\bf H}_{p} = {^p}{\bf F}_s \; {^s}{\bf H}_s\f] The example below shows how to handle that transformation. \code -#include -#include -#include - -int main() -{ -#ifdef VISP_HAVE_VIPER850 - vpRobotViper850 robot; - vpColVector sH = robot.getForceTorque(); // Get the force/torque measures - - // Set the transformation from sensor frame to the probe frame - vpHomogeneousMatrix pMs; - pMs[2][3] = -0.262; // tz only - - // Set the force/torque twist transformation - vpForceTwistMatrix pFs(pMs); // Twist transformation matrix from probe to sensor frame - - // Compute the resulting force/torque in the probe frame - vpColVector pH(6); // Force/torque in the probe frame - pH = pFs * sH; -#endif -} + #include + #include + #include + + int main() + { + #ifdef VISP_HAVE_VIPER850 + vpRobotViper850 robot; + vpColVector sH = robot.getForceTorque(); // Get the force/torque measures + + // Set the transformation from sensor frame to the probe frame + vpHomogeneousMatrix pMs; + pMs[2][3] = -0.262; // tz only + + // Set the force/torque twist transformation + vpForceTwistMatrix pFs(pMs); // Twist transformation matrix from probe to sensor frame + + // Compute the resulting force/torque in the probe frame + vpColVector pH(6); // Force/torque in the probe frame + pH = pFs * sH; + #endif + } \endcode \exception vpException::dimensionError If \f$ \bf H \f$is not a 6 @@ -362,7 +360,8 @@ int main() */ vpColVector vpForceTwistMatrix::operator*(const vpColVector &H) const { - vpColVector Hout(6); + const unsigned int nparam = 6; + vpColVector Hout(nparam); if (6 != H.getRows()) { throw(vpException(vpException::dimensionError, @@ -373,8 +372,8 @@ vpColVector vpForceTwistMatrix::operator*(const vpColVector &H) const Hout = 0.0; - for (unsigned int i = 0; i < 6; ++i) { - for (unsigned int j = 0; j < 6; ++j) { + for (unsigned int i = 0; i < nparam; ++i) { + for (unsigned int j = 0; j < nparam; ++j) { Hout[i] += rowPtrs[i][j] * H[j]; } } @@ -544,8 +543,9 @@ vpForceTwistMatrix &vpForceTwistMatrix::build(const vpTranslationVector &t, cons { vpMatrix skewaR = t.skew(t) * R; - for (unsigned int i = 0; i < 3; ++i) { - for (unsigned int j = 0; j < 3; ++j) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { + for (unsigned int j = 0; j < val_3; ++j) { (*this)[i][j] = R[i][j]; (*this)[i + 3][j + 3] = R[i][j]; (*this)[i + 3][j] = skewaR[i][j]; @@ -572,8 +572,9 @@ vpForceTwistMatrix &vpForceTwistMatrix::build(const vpTranslationVector &t, cons */ vpForceTwistMatrix &vpForceTwistMatrix::build(const vpRotationMatrix &R) { - for (unsigned int i = 0; i < 3; ++i) { - for (unsigned int j = 0; j < 3; ++j) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { + for (unsigned int j = 0; j < val_3; ++j) { (*this)[i][j] = R[i][j]; (*this)[i + 3][j + 3] = R[i][j]; (*this)[i + 3][j] = 0; diff --git a/modules/core/src/math/transformation/vpHomogeneousMatrix.cpp b/modules/core/src/math/transformation/vpHomogeneousMatrix.cpp index d2c9f21daf..78c6f94d9f 100644 --- a/modules/core/src/math/transformation/vpHomogeneousMatrix.cpp +++ b/modules/core/src/math/transformation/vpHomogeneousMatrix.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -39,7 +38,6 @@ the particular case of an homogeneous matrix. */ -#include #include #include #include @@ -51,7 +49,7 @@ BEGIN_VISP_NAMESPACE Construct an homogeneous matrix from a translation vector and quaternion rotation vector. */ -vpHomogeneousMatrix::vpHomogeneousMatrix(const vpTranslationVector &t, const vpQuaternionVector &q) + vpHomogeneousMatrix::vpHomogeneousMatrix(const vpTranslationVector &t, const vpQuaternionVector &q) : vpArray2D(4, 4) { build(t, q); @@ -90,9 +88,10 @@ vpHomogeneousMatrix::vpHomogeneousMatrix(const vpTranslationVector &t, const vpT vpHomogeneousMatrix::vpHomogeneousMatrix(const vpTranslationVector &t, const vpRotationMatrix &R) : vpArray2D(4, 4), m_index(0) { + const unsigned int index_3 = 3; insert(R); insert(t); - (*this)[3][3] = 1.; + (*this)[index_3][index_3] = 1.; } /*! @@ -100,8 +99,14 @@ vpHomogeneousMatrix::vpHomogeneousMatrix(const vpTranslationVector &t, const vpR */ vpHomogeneousMatrix::vpHomogeneousMatrix(const vpPoseVector &p) : vpArray2D(4, 4), m_index(0) { - build(p[0], p[1], p[2], p[3], p[4], p[5]); - (*this)[3][3] = 1.; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + const unsigned int index_5 = 5; + build(p[index_0], p[index_1], p[index_2], p[index_3], p[index_4], p[index_5]); + (*this)[index_3][index_3] = 1.; } /*! @@ -221,19 +226,30 @@ vpHomogeneousMatrix::vpHomogeneousMatrix(const std::initializer_list &li if (!isAnHomogeneousMatrix()) { if (isAnHomogeneousMatrix(1e-3)) { + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + const unsigned int index_5 = 5; + const unsigned int index_6 = 6; + const unsigned int index_7 = 7; + const unsigned int index_8 = 8; + const unsigned int index_9 = 9; + const unsigned int index_10 = 10; // re-orthogonalize rotation matrix since the input is close to a valid rotation matrix vpRotationMatrix R(*this); R.orthogonalize(); - data[0] = R[0][0]; - data[1] = R[0][1]; - data[2] = R[0][2]; - data[4] = R[1][0]; - data[5] = R[1][1]; - data[6] = R[1][2]; - data[8] = R[2][0]; - data[9] = R[2][1]; - data[10] = R[2][2]; + data[index_0] = R[index_0][index_0]; + data[index_1] = R[index_0][index_1]; + data[index_2] = R[index_0][index_2]; + data[index_4] = R[index_1][index_0]; + data[index_5] = R[index_1][index_1]; + data[index_6] = R[index_1][index_2]; + data[index_8] = R[index_2][index_0]; + data[index_9] = R[index_2][index_1]; + data[index_10] = R[index_2][index_2]; } else { throw(vpException( @@ -473,8 +489,14 @@ vpHomogeneousMatrix &vpHomogeneousMatrix::build(const vpTranslationVector &t, co */ vpHomogeneousMatrix &vpHomogeneousMatrix::build(const vpPoseVector &p) { - vpTranslationVector tv(p[0], p[1], p[2]); - vpThetaUVector tu(p[3], p[4], p[5]); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + const unsigned int index_5 = 5; + vpTranslationVector tv(p[index_0], p[index_1], p[index_2]); + vpThetaUVector tu(p[index_3], p[index_4], p[index_5]); insert(tu); insert(tv); @@ -698,7 +720,8 @@ vpHomogeneousMatrix &vpHomogeneousMatrix::operator*=(const vpHomogeneousMatrix & */ vpColVector vpHomogeneousMatrix::operator*(const vpColVector &v) const { - if (v.getRows() != 4) { + const unsigned int val_4 = 4; + if (v.getRows() != val_4) { throw(vpException(vpException::dimensionError, "Cannot multiply a (4x4) homogeneous matrix by a " "(%dx1) column vector", @@ -708,8 +731,8 @@ vpColVector vpHomogeneousMatrix::operator*(const vpColVector &v) const p = 0.0; - for (unsigned int j = 0; j < 4; ++j) { - for (unsigned int i = 0; i < 4; ++i) { + for (unsigned int j = 0; j < val_4; ++j) { + for (unsigned int i = 0; i < val_4; ++i) { p[i] += rowPtrs[i][j] * v[j]; } } @@ -734,28 +757,33 @@ vpPoint vpHomogeneousMatrix::operator*(const vpPoint &bP) const vpColVector v(4), v1(4); - v[0] = bP.get_X(); - v[1] = bP.get_Y(); - v[2] = bP.get_Z(); - v[3] = bP.get_W(); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + + v[index_0] = bP.get_X(); + v[index_1] = bP.get_Y(); + v[index_2] = bP.get_Z(); + v[index_3] = bP.get_W(); - v1[0] = ((*this)[0][0] * v[0]) + ((*this)[0][1] * v[1]) + ((*this)[0][2] * v[2]) + ((*this)[0][3] * v[3]); - v1[1] = ((*this)[1][0] * v[0]) + ((*this)[1][1] * v[1]) + ((*this)[1][2] * v[2]) + ((*this)[1][3] * v[3]); - v1[2] = ((*this)[2][0] * v[0]) + ((*this)[2][1] * v[1]) + ((*this)[2][2] * v[2]) + ((*this)[2][3] * v[3]); - v1[3] = ((*this)[3][0] * v[0]) + ((*this)[3][1] * v[1]) + ((*this)[3][2] * v[2]) + ((*this)[3][3] * v[3]); + v1[index_0] = ((*this)[index_0][0] * v[0]) + ((*this)[index_0][1] * v[1]) + ((*this)[index_0][index_2] * v[index_2]) + ((*this)[index_0][index_3] * v[index_3]); + v1[index_1] = ((*this)[index_1][0] * v[0]) + ((*this)[index_1][1] * v[1]) + ((*this)[index_1][index_2] * v[index_2]) + ((*this)[index_1][index_3] * v[index_3]); + v1[index_2] = ((*this)[index_2][0] * v[0]) + ((*this)[index_2][1] * v[1]) + ((*this)[index_2][index_2] * v[index_2]) + ((*this)[index_2][index_3] * v[index_3]); + v1[index_3] = ((*this)[index_3][0] * v[0]) + ((*this)[index_3][1] * v[1]) + ((*this)[index_3][index_2] * v[index_2]) + ((*this)[index_3][index_3] * v[index_3]); v1 /= v1[3]; // --comment: v1 equals M multiplied by v - aP.set_X(v1[0]); - aP.set_Y(v1[1]); - aP.set_Z(v1[2]); - aP.set_W(v1[3]); + aP.set_X(v1[index_0]); + aP.set_Y(v1[index_1]); + aP.set_Z(v1[index_2]); + aP.set_W(v1[index_3]); - aP.set_oX(v1[0]); - aP.set_oY(v1[1]); - aP.set_oZ(v1[2]); - aP.set_oW(v1[3]); + aP.set_oX(v1[index_0]); + aP.set_oY(v1[index_1]); + aP.set_oZ(v1[index_2]); + aP.set_oW(v1[index_3]); return aP; } @@ -773,9 +801,13 @@ vpPoint vpHomogeneousMatrix::operator*(const vpPoint &bP) const vpTranslationVector vpHomogeneousMatrix::operator*(const vpTranslationVector &t) const { vpTranslationVector t_out; - t_out[0] = (((*this)[0][0] * t[0]) + ((*this)[0][1] * t[1]) + ((*this)[0][2] * t[2])) + (*this)[0][3]; - t_out[1] = (((*this)[1][0] * t[0]) + ((*this)[1][1] * t[1]) + ((*this)[1][2] * t[2])) + (*this)[1][3]; - t_out[2] = (((*this)[2][0] * t[0]) + ((*this)[2][1] * t[1]) + ((*this)[2][2] * t[2])) + (*this)[2][3]; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + t_out[index_0] = (((*this)[index_0][0] * t[0]) + ((*this)[index_0][1] * t[1]) + ((*this)[index_0][index_2] * t[index_2])) + (*this)[index_0][index_3]; + t_out[index_1] = (((*this)[index_1][0] * t[0]) + ((*this)[index_1][1] * t[1]) + ((*this)[index_1][index_2] * t[index_2])) + (*this)[index_1][index_3]; + t_out[index_2] = (((*this)[index_2][0] * t[0]) + ((*this)[index_2][1] * t[1]) + ((*this)[index_2][index_2] * t[index_2])) + (*this)[index_2][index_3]; return t_out; } @@ -916,9 +948,13 @@ bool vpHomogeneousMatrix::isAnHomogeneousMatrix(double threshold) const vpRotationMatrix R; extract(R); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; const double epsilon = std::numeric_limits::epsilon(); - return R.isARotationMatrix(threshold) && vpMath::nul((*this)[3][0], epsilon) && vpMath::nul((*this)[3][1], epsilon) && - vpMath::nul((*this)[3][2], epsilon) && vpMath::equal((*this)[3][3], 1.0, epsilon); + return R.isARotationMatrix(threshold) && vpMath::nul((*this)[index_3][index_0], epsilon) && vpMath::nul((*this)[index_3][index_1], epsilon) && + vpMath::nul((*this)[index_3][index_2], epsilon) && vpMath::equal((*this)[index_3][index_3], 1.0, epsilon); } /*! @@ -942,8 +978,9 @@ bool vpHomogeneousMatrix::isValid() const */ void vpHomogeneousMatrix::extract(vpRotationMatrix &R) const { - for (unsigned int i = 0; i < 3; ++i) { - for (unsigned int j = 0; j < 3; ++j) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { + for (unsigned int j = 0; j < val_3; ++j) { R[i][j] = (*this)[i][j]; } } @@ -954,9 +991,13 @@ void vpHomogeneousMatrix::extract(vpRotationMatrix &R) const */ void vpHomogeneousMatrix::extract(vpTranslationVector &t) const { - t[0] = (*this)[0][3]; - t[1] = (*this)[1][3]; - t[2] = (*this)[2][3]; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + t[index_0] = (*this)[index_0][index_3]; + t[index_1] = (*this)[index_1][index_3]; + t[index_2] = (*this)[index_2][index_3]; } /*! Extract the rotation as a \f$\theta \bf u\f$ vector. @@ -983,8 +1024,9 @@ void vpHomogeneousMatrix::extract(vpQuaternionVector &q) const */ void vpHomogeneousMatrix::insert(const vpRotationMatrix &R) { - for (unsigned int i = 0; i < 3; ++i) { - for (unsigned int j = 0; j < 3; ++j) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { + for (unsigned int j = 0; j < val_3; ++j) { (*this)[i][j] = R[i][j]; } } @@ -1007,9 +1049,13 @@ void vpHomogeneousMatrix::insert(const vpThetaUVector &tu) */ void vpHomogeneousMatrix::insert(const vpTranslationVector &t) { - (*this)[0][3] = t[0]; - (*this)[1][3] = t[1]; - (*this)[2][3] = t[2]; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + (*this)[index_0][index_3] = t[index_0]; + (*this)[index_1][index_3] = t[index_1]; + (*this)[index_2][index_3] = t[index_2]; } /*! @@ -1057,23 +1103,27 @@ vpHomogeneousMatrix vpHomogeneousMatrix::inverse() const */ void vpHomogeneousMatrix::eye() { - (*this)[0][0] = 1; - (*this)[1][1] = 1; - (*this)[2][2] = 1; - (*this)[3][3] = 1; - - (*this)[0][1] = 0; - (*this)[0][2] = 0; - (*this)[0][3] = 0; - (*this)[1][0] = 0; - (*this)[1][2] = 0; - (*this)[1][3] = 0; - (*this)[2][0] = 0; - (*this)[2][1] = 0; - (*this)[2][3] = 0; - (*this)[3][0] = 0; - (*this)[3][1] = 0; - (*this)[3][2] = 0; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + (*this)[index_0][index_0] = 1; + (*this)[index_1][index_1] = 1; + (*this)[index_2][index_2] = 1; + (*this)[index_3][index_3] = 1; + + (*this)[index_0][index_1] = 0; + (*this)[index_0][index_2] = 0; + (*this)[index_0][index_3] = 0; + (*this)[index_1][index_0] = 0; + (*this)[index_1][index_2] = 0; + (*this)[index_1][index_3] = 0; + (*this)[index_2][index_0] = 0; + (*this)[index_2][index_1] = 0; + (*this)[index_2][index_3] = 0; + (*this)[index_3][index_0] = 0; + (*this)[index_3][index_1] = 0; + (*this)[index_3][index_2] = 0; } /*! @@ -1113,8 +1163,9 @@ void vpHomogeneousMatrix::save(const std::string &filename) const void vpHomogeneousMatrix::load(std::ifstream &f) { if (!f.fail()) { - for (unsigned int i = 0; i < 4; ++i) { - for (unsigned int j = 0; j < 4; ++j) { + const unsigned int val_4 = 4; + for (unsigned int i = 0; i < val_4; ++i) { + for (unsigned int j = 0; j < val_4; ++j) { f >> (*this)[i][j]; } } @@ -1140,15 +1191,25 @@ void vpHomogeneousMatrix::orthogonalizeRotation() vpRotationMatrix R(*this); R.orthogonalize(); - data[0] = R[0][0]; - data[1] = R[0][1]; - data[2] = R[0][2]; - data[4] = R[1][0]; - data[5] = R[1][1]; - data[6] = R[1][2]; - data[8] = R[2][0]; - data[9] = R[2][1]; - data[10] = R[2][2]; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_4 = 4; + const unsigned int index_5 = 5; + const unsigned int index_6 = 6; + const unsigned int index_8 = 8; + const unsigned int index_9 = 9; + const unsigned int index_10 = 10; + + data[index_0] = R[index_0][index_0]; + data[index_1] = R[index_0][index_1]; + data[index_2] = R[index_0][index_2]; + data[index_4] = R[index_1][index_0]; + data[index_5] = R[index_1][index_1]; + data[index_6] = R[index_1][index_2]; + data[index_8] = R[index_2][index_0]; + data[index_9] = R[index_2][index_1]; + data[index_10] = R[index_2][index_2]; } //! Print the matrix as a pose vector \f$({\bf t}^T \theta {\bf u}^T)\f$ @@ -1267,14 +1328,15 @@ vpHomogeneousMatrix vpHomogeneousMatrix::compute3d3dTransformation(const std::ve vpColVector p_bar(3, 0.0); vpColVector q_bar(3, 0.0); size_t p_size = p.size(); + const unsigned int val_3 = 3; for (size_t i = 0; i < p_size; ++i) { - for (unsigned int j = 0; j < 3; ++j) { + for (unsigned int j = 0; j < val_3; ++j) { p_bar[j] += p.at(i).oP[j]; q_bar[j] += q.at(i).oP[j]; } } - for (unsigned int j = 0; j < 3; ++j) { + for (unsigned int j = 0; j < val_3; ++j) { p_bar[j] /= N; q_bar[j] /= N; } @@ -1283,7 +1345,7 @@ vpHomogeneousMatrix vpHomogeneousMatrix::compute3d3dTransformation(const std::ve vpMatrix qc(static_cast(q.size()), 3); for (unsigned int i = 0; i < static_cast(p_size); ++i) { - for (unsigned int j = 0; j < 3; ++j) { + for (unsigned int j = 0; j < val_3; ++j) { pc[i][j] = p.at(i).oP[j] - p_bar[j]; qc[i][j] = q.at(i).oP[j] - q_bar[j]; } @@ -1297,9 +1359,12 @@ vpHomogeneousMatrix vpHomogeneousMatrix::compute3d3dTransformation(const std::ve vpMatrix Vt = V.t(); vpMatrix R = U * Vt; if (R.det() < 0) { - Vt[2][0] *= -1; - Vt[2][1] *= -1; - Vt[2][2] *= -1; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + Vt[index_2][index_0] *= -1.; + Vt[index_2][index_1] *= -1.; + Vt[index_2][index_2] *= -1.; R = U * Vt; } @@ -1323,6 +1388,9 @@ vpHomogeneousMatrix vpHomogeneousMatrix::mean(const std::vector 0) { meanR = U * V.t(); } else { vpMatrix D(3, 3); D = 0.0; - D[0][0] = 1.0; - D[1][1] = 1.0; - D[2][2] = -1; + D[index_0][index_0] = 1.0; + D[index_1][index_1] = 1.0; + D[index_2][index_2] = -1.; meanR = U * D * V.t(); } diff --git a/modules/core/src/math/transformation/vpPoseVector.cpp b/modules/core/src/math/transformation/vpPoseVector.cpp index 5c73b4c935..1abff87cfd 100644 --- a/modules/core/src/math/transformation/vpPoseVector.cpp +++ b/modules/core/src/math/transformation/vpPoseVector.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -43,7 +42,6 @@ #include #include -#include #include #include #include @@ -81,13 +79,19 @@ vpPoseVector::vpPoseVector() : vpArray2D(6, 1) { } vpPoseVector::vpPoseVector(double tx, double ty, double tz, double tux, double tuy, double tuz) : vpArray2D(6, 1) { - (*this)[0] = tx; - (*this)[1] = ty; - (*this)[2] = tz; - - (*this)[3] = tux; - (*this)[4] = tuy; - (*this)[5] = tuz; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + const unsigned int index_5 = 5; + (*this)[index_0] = tx; + (*this)[index_1] = ty; + (*this)[index_2] = tz; + + (*this)[index_3] = tux; + (*this)[index_4] = tuy; + (*this)[index_5] = tuz; } /*! @@ -151,13 +155,19 @@ vpPoseVector::vpPoseVector(const vpHomogeneousMatrix &M) : vpArray2D(6, */ void vpPoseVector::set(double tx, double ty, double tz, double tux, double tuy, double tuz) { - (*this)[0] = tx; - (*this)[1] = ty; - (*this)[2] = tz; - - (*this)[3] = tux; - (*this)[4] = tuy; - (*this)[5] = tuz; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + const unsigned int index_5 = 5; + (*this)[index_0] = tx; + (*this)[index_1] = ty; + (*this)[index_2] = tz; + + (*this)[index_3] = tux; + (*this)[index_4] = tuy; + (*this)[index_5] = tuz; } #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS @@ -257,13 +267,19 @@ vpPoseVector vpPoseVector::buildFrom(const vpTranslationVector &tv, const vpRota */ vpPoseVector &vpPoseVector::build(const double &tx, const double &ty, const double &tz, const double &tux, const double &tuy, const double &tuz) { - (*this)[0] = tx; - (*this)[1] = ty; - (*this)[2] = tz; - - (*this)[3] = tux; - (*this)[4] = tuy; - (*this)[5] = tuz; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + const unsigned int index_5 = 5; + (*this)[index_0] = tx; + (*this)[index_1] = ty; + (*this)[index_2] = tz; + + (*this)[index_3] = tux; + (*this)[index_4] = tuy; + (*this)[index_5] = tuz; return *this; } @@ -301,7 +317,8 @@ vpPoseVector &vpPoseVector::build(const vpHomogeneousMatrix &M) */ vpPoseVector &vpPoseVector::build(const vpTranslationVector &tv, const vpThetaUVector &tu) { - for (unsigned int i = 0; i < 3; ++i) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { (*this)[i] = tv[i]; (*this)[i + 3] = tu[i]; } @@ -335,9 +352,12 @@ vpPoseVector &vpPoseVector::build(const vpTranslationVector &tv, const vpRotatio */ void vpPoseVector::extract(vpTranslationVector &tv) const { - tv[0] = (*this)[0]; - tv[1] = (*this)[1]; - tv[2] = (*this)[2]; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + tv[index_0] = (*this)[index_0]; + tv[index_1] = (*this)[index_1]; + tv[index_2] = (*this)[index_2]; } /*! @@ -345,9 +365,15 @@ void vpPoseVector::extract(vpTranslationVector &tv) const */ void vpPoseVector::extract(vpThetaUVector &tu) const { - tu[0] = (*this)[3]; - tu[1] = (*this)[4]; - tu[2] = (*this)[5]; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + const unsigned int index_5 = 5; + tu[index_0] = (*this)[index_3]; + tu[index_1] = (*this)[index_4]; + tu[index_2] = (*this)[index_5]; } /*! Extract the rotation as a quaternion vector. @@ -367,7 +393,10 @@ void vpPoseVector::extract(vpRotationMatrix &R) const { R.build((*this)[3], (*th */ vpTranslationVector vpPoseVector::getTranslationVector() const { - vpTranslationVector tr((*this)[0], (*this)[1], (*this)[2]); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + vpTranslationVector tr((*this)[index_0], (*this)[index_1], (*this)[index_2]); return tr; } @@ -414,8 +443,10 @@ vpThetaUVector vpPoseVector::getThetaUVector() const */ void vpPoseVector::print() const { - for (unsigned int i = 0; i < 6; ++i) { - if (i < 3) { + const unsigned int nparam = 6; + const unsigned int nparam_t = 3; + for (unsigned int i = 0; i < nparam; ++i) { + if (i < nparam_t) { std::cout << (*this)[i] << " "; } else { @@ -459,7 +490,8 @@ void vpPoseVector::save(std::ofstream &f) const void vpPoseVector::load(std::ifstream &f) { if (!f.fail()) { - for (unsigned int i = 0; i < 6; ++i) { + const unsigned int nparam = 6; + for (unsigned int i = 0; i < nparam; ++i) { f >> (*this)[i]; } } diff --git a/modules/core/src/math/transformation/vpQuaternionVector.cpp b/modules/core/src/math/transformation/vpQuaternionVector.cpp index 8d845db705..bdaee5ddea 100644 --- a/modules/core/src/math/transformation/vpQuaternionVector.cpp +++ b/modules/core/src/math/transformation/vpQuaternionVector.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,7 @@ * * Description: * Quaternion vector. - * - * Authors: - * Filip Novotny - * -*****************************************************************************/ + */ #include #include @@ -94,10 +89,14 @@ vpQuaternionVector::vpQuaternionVector(const vpThetaUVector &tu) : vpRotationVec */ void vpQuaternionVector::set(double qx, double qy, double qz, double qw) { - data[0] = qx; - data[1] = qy; - data[2] = qz; - data[3] = qw; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + data[index_0] = qx; + data[index_1] = qy; + data[index_2] = qz; + data[index_3] = qw; } #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS @@ -201,7 +200,8 @@ vpQuaternionVector &vpQuaternionVector::build(const vpColVector &q) throw(vpException(vpException::dimensionError, "Cannot construct a quaternion vector from a %d-dimension col vector", q.size())); } - for (unsigned int i = 0; i < 4; ++i) { + const unsigned int val_4 = 4; + for (unsigned int i = 0; i < val_4; ++i) { data[i] = q[i]; } @@ -217,7 +217,9 @@ vpQuaternionVector &vpQuaternionVector::build(const std::vector &q) throw(vpException(vpException::dimensionError, "Cannot construct a quaternion vector from a %d-dimension std::vector", q.size())); } - for (unsigned int i = 0; i < 4; ++i) { + + const unsigned int val_4 = 4; + for (unsigned int i = 0; i < val_4; ++i) { data[i] = q[i]; } @@ -239,7 +241,10 @@ vpQuaternionVector &vpQuaternionVector::build(const vpRotationMatrix &R) theta *= 0.5; double sinTheta_2 = sin(theta); - set(u[0] * sinTheta_2, u[1] * sinTheta_2, u[2] * sinTheta_2, cos(theta)); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + set(u[index_0] * sinTheta_2, u[index_1] * sinTheta_2, u[index_2] * sinTheta_2, cos(theta)); return *this; } @@ -294,28 +299,26 @@ vpQuaternionVector vpQuaternionVector::operator/(double l) const return vpQuaternionVector(x() / l, y() / l, z() / l, w() / l); } /*! - Copy operator that initializes a quaternion vector from a 4-dimension column -vector \e q. - - \param q : 4-dimension vector containing the values of the quaternion -vector. + vector \e q. -\code -#include + \param q : 4-dimension vector containing the values of the quaternion vector. -int main() -{ - vpColVector v(4); - v[0] = 0.1; - v[1] = 0.2; - v[2] = 0.3; - v[3] = 0.4; - vpQuaternionVector q; - q = v; - // q is now equal to v : 0.1, 0.2, 0.3, 0.4 -} -\endcode + \code + #include + + int main() + { + vpColVector v(4); + v[0] = 0.1; + v[1] = 0.2; + v[2] = 0.3; + v[3] = 0.4; + vpQuaternionVector q; + q = v; + // q is now equal to v : 0.1, 0.2, 0.3, 0.4 + } + \endcode */ vpQuaternionVector &vpQuaternionVector::operator=(const vpColVector &q) { @@ -323,7 +326,8 @@ vpQuaternionVector &vpQuaternionVector::operator=(const vpColVector &q) throw(vpException(vpException::dimensionError, "Cannot set a quaternion vector from a %d-dimension col vector", q.size())); } - for (unsigned int i = 0; i < 4; ++i) { + const unsigned int val_4 = 4; + for (unsigned int i = 0; i < val_4; ++i) { data[i] = q[i]; } @@ -391,38 +395,38 @@ double vpQuaternionVector::dot(const vpQuaternionVector &q0, const vpQuaternionV } //! Returns the x-component of the quaternion. -const double &vpQuaternionVector::x() const { return data[0]; } +const double &vpQuaternionVector::x() const { const unsigned int index_0 = 0; return data[index_0]; } //! Returns the y-component of the quaternion. -const double &vpQuaternionVector::y() const { return data[1]; } +const double &vpQuaternionVector::y() const { const unsigned int index_1 = 1; return data[index_1]; } //! Returns the z-component of the quaternion. -const double &vpQuaternionVector::z() const { return data[2]; } +const double &vpQuaternionVector::z() const { const unsigned int index_2 = 2; return data[index_2]; } //! Returns the w-component of the quaternion. -const double &vpQuaternionVector::w() const { return data[3]; } +const double &vpQuaternionVector::w() const { const unsigned int index_3 = 3; return data[index_3]; } //! Returns a reference to the x-component of the quaternion. -double &vpQuaternionVector::x() { return data[0]; } +double &vpQuaternionVector::x() { const unsigned int index_0 = 0; return data[index_0]; } //! Returns a reference to the y-component of the quaternion. -double &vpQuaternionVector::y() { return data[1]; } +double &vpQuaternionVector::y() { const unsigned int index_1 = 1; return data[index_1]; } //! Returns a reference to the z-component of the quaternion. -double &vpQuaternionVector::z() { return data[2]; } +double &vpQuaternionVector::z() { const unsigned int index_2 = 2; return data[index_2]; } //! Returns a reference to the w-component of the quaternion. -double &vpQuaternionVector::w() { return data[3]; } +double &vpQuaternionVector::w() { const unsigned int index_3 = 3; return data[index_3]; } #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) /*! Set vector from a list of 4 double angle values. \code -#include + #include -int main() -{ - vpQuaternionVector q = {0, 0, 0, 1}; - std::cout << "q: " << q.t() << std::endl; -} + int main() + { + vpQuaternionVector q = {0, 0, 0, 1}; + std::cout << "q: " << q.t() << std::endl; + } \endcode It produces the following printings: \code -q: 0 0 0 1 + q: 0 0 0 1 \endcode \sa operator<<() */ diff --git a/modules/core/src/math/transformation/vpRotationMatrix.cpp b/modules/core/src/math/transformation/vpRotationMatrix.cpp index 6f5927d890..b1110e9368 100644 --- a/modules/core/src/math/transformation/vpRotationMatrix.cpp +++ b/modules/core/src/math/transformation/vpRotationMatrix.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Rotation matrix. - * -*****************************************************************************/ + */ /*! \file vpRotationMatrix.cpp @@ -50,7 +48,6 @@ // Debug trace #include -#include BEGIN_VISP_NAMESPACE /*! @@ -60,8 +57,9 @@ BEGIN_VISP_NAMESPACE */ void vpRotationMatrix::eye() { - for (unsigned int i = 0; i < 3; ++i) { - for (unsigned int j = 0; j < 3; ++j) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { + for (unsigned int j = 0; j < val_3; ++j) { if (i == j) { (*this)[i][j] = 1.0; } @@ -83,8 +81,9 @@ void vpRotationMatrix::eye() */ vpRotationMatrix &vpRotationMatrix::operator=(const vpRotationMatrix &R) { - for (unsigned int i = 0; i < 3; ++i) { - for (unsigned int j = 0; j < 3; ++j) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { + for (unsigned int j = 0; j < val_3; ++j) { rowPtrs[i][j] = R.rowPtrs[i][j]; } } @@ -163,8 +162,9 @@ vpRotationMatrix &vpRotationMatrix::operator=(const vpMatrix &M) M.getRows(), M.getCols())); } - for (unsigned int i = 0; i < 3; ++i) { - for (unsigned int j = 0; j < 3; ++j) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { + for (unsigned int j = 0; j < val_3; ++j) { (*this)[i][j] = M[i][j]; } } @@ -186,21 +186,21 @@ vpRotationMatrix &vpRotationMatrix::operator=(const vpMatrix &M) The following example shows how to initialize a rotation matrix using this operator. \code -#include + #include -int main() -{ - vpRotationMatrix R; - R << 0, 0, -1, 0, -1, 0, -1, 0, 0; - std::cout << "R:\n" << R << std::endl; -} + int main() + { + vpRotationMatrix R; + R << 0, 0, -1, 0, -1, 0, -1, 0, 0; + std::cout << "R:\n" << R << std::endl; + } \endcode It produces the following printings: \code -R: -0 0 -1 -0 -1 0 --1 0 0 + R: + 0 0 -1 + 0 -1 0 + -1 0 0 \endcode \sa operator,() @@ -220,21 +220,21 @@ vpRotationMatrix &vpRotationMatrix::operator<<(double val) The following example shows how to initialize a rotation matrix using this operator. \code -#include + #include -int main() -{ - vpRotationMatrix R; - R << 0, 0, -1, 0, -1, 0, -1, 0, 0; - std::cout << "R:\n" << R << std::endl; -} + int main() + { + vpRotationMatrix R; + R << 0, 0, -1, 0, -1, 0, -1, 0, 0; + std::cout << "R:\n" << R << std::endl; + } \endcode It produces the following printings: \code -R: -0 0 -1 -0 -1 0 --1 0 0 + R: + 0 0 -1 + 0 -1 0 + -1 0 0 \endcode \sa operator<<() @@ -259,8 +259,9 @@ vpRotationMatrix vpRotationMatrix::operator*(const vpRotationMatrix &R) const { vpRotationMatrix p; - for (unsigned int i = 0; i < 3; ++i) { - for (unsigned int j = 0; j < 3; ++j) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { + for (unsigned int j = 0; j < val_3; ++j) { double s = 0; for (unsigned int k = 0; k < 3; ++k) { s += rowPtrs[i][k] * R.rowPtrs[k][j]; @@ -296,8 +297,9 @@ vpMatrix vpRotationMatrix::operator*(const vpMatrix &M) const } vpMatrix p(3, 3); - for (unsigned int i = 0; i < 3; ++i) { - for (unsigned int j = 0; j < 3; ++j) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { + for (unsigned int j = 0; j < val_3; ++j) { double s = 0; for (unsigned int k = 0; k < 3; ++k) { s += (*this)[i][k] * M[k][j]; @@ -340,25 +342,26 @@ vpHomogeneousMatrix vpRotationMatrix::operator*(const vpHomogeneousMatrix &M) co vector \e v is not a 3 dimension vector. The code below shows how to use this operator. -\code -#include -#include + \code + #include + #include -int main() -{ - vpColVector p1(3), p2(3); - vpRotationMatrix R; + int main() + { + vpColVector p1(3), p2(3); + vpRotationMatrix R; - p2 = R * p1; + p2 = R * p1; - return 0; -} -\endcode + return 0; + } + \endcode */ vpColVector vpRotationMatrix::operator*(const vpColVector &v) const { - if (v.getRows() != 3) { + const unsigned int rows_size = 3; + if (v.getRows() != rows_size) { throw(vpException(vpException::dimensionError, "Cannot multiply a (3x3) rotation matrix by a %d " "dimension column vector", @@ -383,13 +386,14 @@ vpColVector vpRotationMatrix::operator*(const vpColVector &v) const vpTranslationVector vpRotationMatrix::operator*(const vpTranslationVector &tv) const { vpTranslationVector p; + const unsigned int val_3 = 3; - for (unsigned int j = 0; j < 3; ++j) { + for (unsigned int j = 0; j < val_3; ++j) { p[j] = 0; } - for (unsigned int j = 0; j < 3; ++j) { - for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int j = 0; j < val_3; ++j) { + for (unsigned int i = 0; i < val_3; ++i) { p[i] += rowPtrs[i][j] * tv[j]; } } @@ -447,8 +451,9 @@ bool vpRotationMatrix::isARotationMatrix(double threshold) const // --comment: test R^TR = Id vpRotationMatrix RtR = (*this).t() * (*this); - for (unsigned int i = 0; i < 3; ++i) { - for (unsigned int j = 0; j < 3; ++j) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { + for (unsigned int j = 0; j < val_3; ++j) { if (i == j) { if (fabs(RtR[i][j] - 1) > threshold) { isRotation = false; @@ -463,15 +468,16 @@ bool vpRotationMatrix::isARotationMatrix(double threshold) const } // test if it is a basis // test || Ci || = 1 - for (unsigned int i = 0; i < 3; ++i) { - if ((sqrt(vpMath::sqr(RtR[0][i]) + vpMath::sqr(RtR[1][i]) + vpMath::sqr(RtR[2][i])) - 1) > threshold) { + const unsigned int index_2 = 2; + for (unsigned int i = 0; i < val_3; ++i) { + if ((sqrt(vpMath::sqr(RtR[0][i]) + vpMath::sqr(RtR[1][i]) + vpMath::sqr(RtR[index_2][i])) - 1) > threshold) { isRotation = false; } } // test || Ri || = 1 - for (unsigned int i = 0; i < 3; ++i) { - if ((sqrt(vpMath::sqr(RtR[i][0]) + vpMath::sqr(RtR[i][1]) + vpMath::sqr(RtR[i][2])) - 1) > threshold) { + for (unsigned int i = 0; i < val_3; ++i) { + if ((sqrt(vpMath::sqr(RtR[i][0]) + vpMath::sqr(RtR[i][1]) + vpMath::sqr(RtR[i][index_2])) - 1) > threshold) { isRotation = false; } } @@ -595,8 +601,9 @@ vpRotationMatrix vpRotationMatrix::t() const { vpRotationMatrix Rt; - for (unsigned int i = 0; i < 3; ++i) { - for (unsigned int j = 0; j < 3; ++j) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { + for (unsigned int j = 0; j < val_3; ++j) { Rt[j][i] = (*this)[i][j]; } } @@ -644,7 +651,8 @@ void vpRotationMatrix::printVector() { vpThetaUVector tu(*this); - for (unsigned int i = 0; i < 3; ++i) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { std::cout << tu[i] << " "; } @@ -766,24 +774,28 @@ vpRotationMatrix &vpRotationMatrix::build(const vpThetaUVector &v) double theta, si, co, sinc, mcosc; vpRotationMatrix R; - theta = sqrt((v[0] * v[0]) + (v[1] * v[1]) + (v[2] * v[2])); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + theta = sqrt((v[index_0] * v[index_0]) + (v[index_1] * v[index_1]) + (v[index_2] * v[index_2])); si = sin(theta); co = cos(theta); sinc = vpMath::sinc(si, theta); mcosc = vpMath::mcosc(co, theta); - R[0][0] = co + (mcosc * v[0] * v[0]); - R[0][1] = (-sinc * v[2]) + (mcosc * v[0] * v[1]); - R[0][2] = (sinc * v[1]) + (mcosc * v[0] * v[2]); - R[1][0] = (sinc * v[2]) + (mcosc * v[1] * v[0]); - R[1][1] = co + (mcosc * v[1] * v[1]); - R[1][2] = (-sinc * v[0]) + (mcosc * v[1] * v[2]); - R[2][0] = (-sinc * v[1]) + (mcosc * v[2] * v[0]); - R[2][1] = (sinc * v[0]) + (mcosc * v[2] * v[1]); - R[2][2] = co + (mcosc * v[2] * v[2]); - - for (unsigned int i = 0; i < 3; ++i) { - for (unsigned int j = 0; j < 3; ++j) { + R[index_0][index_0] = co + (mcosc * v[index_0] * v[index_0]); + R[index_0][index_1] = (-sinc * v[index_2]) + (mcosc * v[index_0] * v[index_1]); + R[index_0][index_2] = (sinc * v[index_1]) + (mcosc * v[index_0] * v[index_2]); + R[index_1][index_0] = (sinc * v[index_2]) + (mcosc * v[index_1] * v[index_0]); + R[index_1][index_1] = co + (mcosc * v[index_1] * v[index_1]); + R[index_1][index_2] = (-sinc * v[index_0]) + (mcosc * v[index_1] * v[index_2]); + R[index_2][index_0] = (-sinc * v[index_1]) + (mcosc * v[index_2] * v[index_0]); + R[index_2][index_1] = (sinc * v[index_0]) + (mcosc * v[index_2] * v[index_1]); + R[index_2][index_2] = co + (mcosc * v[index_2] * v[index_2]); + + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { + for (unsigned int j = 0; j < val_3; ++j) { (*this)[i][j] = R[i][j]; } } @@ -796,8 +808,9 @@ vpRotationMatrix &vpRotationMatrix::build(const vpThetaUVector &v) */ vpRotationMatrix &vpRotationMatrix::build(const vpHomogeneousMatrix &M) { - for (unsigned int i = 0; i < 3; ++i) { - for (unsigned int j = 0; j < 3; ++j) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { + for (unsigned int j = 0; j < val_3; ++j) { (*this)[i][j] = M[i][j]; } } @@ -826,23 +839,26 @@ vpRotationMatrix &vpRotationMatrix::build(const vpPoseVector &p) vpRotationMatrix &vpRotationMatrix::build(const vpRzyzVector &v) { double c0, c1, c2, s0, s1, s2; - - c0 = cos(v[0]); - c1 = cos(v[1]); - c2 = cos(v[2]); - s0 = sin(v[0]); - s1 = sin(v[1]); - s2 = sin(v[2]); - - (*this)[0][0] = (c0 * c1 * c2) - (s0 * s2); - (*this)[0][1] = (-c0 * c1 * s2) - (s0 * c2); - (*this)[0][2] = c0 * s1; - (*this)[1][0] = (s0 * c1 * c2) + (c0 * s2); - (*this)[1][1] = (-s0 * c1 * s2) + (c0 * c2); - (*this)[1][2] = s0 * s1; - (*this)[2][0] = -s1 * c2; - (*this)[2][1] = s1 * s2; - (*this)[2][2] = c1; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + + c0 = cos(v[index_0]); + c1 = cos(v[index_1]); + c2 = cos(v[index_2]); + s0 = sin(v[index_0]); + s1 = sin(v[index_1]); + s2 = sin(v[index_2]); + + (*this)[index_0][index_0] = (c0 * c1 * c2) - (s0 * s2); + (*this)[index_0][index_1] = (-c0 * c1 * s2) - (s0 * c2); + (*this)[index_0][index_2] = c0 * s1; + (*this)[index_1][index_0] = (s0 * c1 * c2) + (c0 * s2); + (*this)[index_1][index_1] = (-s0 * c1 * s2) + (c0 * c2); + (*this)[index_1][index_2] = s0 * s1; + (*this)[index_2][index_0] = -s1 * c2; + (*this)[index_2][index_1] = s1 * s2; + (*this)[index_2][index_2] = c1; return *this; } @@ -858,23 +874,26 @@ vpRotationMatrix &vpRotationMatrix::build(const vpRzyzVector &v) vpRotationMatrix &vpRotationMatrix::build(const vpRxyzVector &v) { double c0, c1, c2, s0, s1, s2; - - c0 = cos(v[0]); - c1 = cos(v[1]); - c2 = cos(v[2]); - s0 = sin(v[0]); - s1 = sin(v[1]); - s2 = sin(v[2]); - - (*this)[0][0] = c1 * c2; - (*this)[0][1] = -c1 * s2; - (*this)[0][2] = s1; - (*this)[1][0] = (c0 * s2) + (s0 * s1 * c2); - (*this)[1][1] = (c0 * c2) - (s0 * s1 * s2); - (*this)[1][2] = -s0 * c1; - (*this)[2][0] = (-c0 * s1 * c2) + (s0 * s2); - (*this)[2][1] = (c0 * s1 * s2) + (c2 * s0); - (*this)[2][2] = c0 * c1; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + + c0 = cos(v[index_0]); + c1 = cos(v[index_1]); + c2 = cos(v[index_2]); + s0 = sin(v[index_0]); + s1 = sin(v[index_1]); + s2 = sin(v[index_2]); + + (*this)[index_0][index_0] = c1 * c2; + (*this)[index_0][index_1] = -c1 * s2; + (*this)[index_0][index_2] = s1; + (*this)[index_1][index_0] = (c0 * s2) + (s0 * s1 * c2); + (*this)[index_1][index_1] = (c0 * c2) - (s0 * s1 * s2); + (*this)[index_1][index_2] = -s0 * c1; + (*this)[index_2][index_0] = (-c0 * s1 * c2) + (s0 * s2); + (*this)[index_2][index_1] = (c0 * s1 * s2) + (c2 * s0); + (*this)[index_2][index_2] = c0 * c1; return *this; } @@ -888,25 +907,28 @@ vpRotationMatrix &vpRotationMatrix::build(const vpRxyzVector &v) vpRotationMatrix &vpRotationMatrix::build(const vpRzyxVector &v) { double c0, c1, c2, s0, s1, s2; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; - c0 = cos(v[0]); - c1 = cos(v[1]); - c2 = cos(v[2]); - s0 = sin(v[0]); - s1 = sin(v[1]); - s2 = sin(v[2]); + c0 = cos(v[index_0]); + c1 = cos(v[index_1]); + c2 = cos(v[index_2]); + s0 = sin(v[index_0]); + s1 = sin(v[index_1]); + s2 = sin(v[index_2]); - (*this)[0][0] = c0 * c1; - (*this)[0][1] = (c0 * s1 * s2) - (s0 * c2); - (*this)[0][2] = (c0 * s1 * c2) + (s0 * s2); + (*this)[index_0][index_0] = c0 * c1; + (*this)[index_0][index_1] = (c0 * s1 * s2) - (s0 * c2); + (*this)[index_0][index_2] = (c0 * s1 * c2) + (s0 * s2); - (*this)[1][0] = s0 * c1; - (*this)[1][1] = (s0 * s1 * s2) + (c0 * c2); - (*this)[1][2] = (s0 * s1 * c2) - (c0 * s2); + (*this)[index_1][index_0] = s0 * c1; + (*this)[index_1][index_1] = (s0 * s1 * s2) + (c0 * c2); + (*this)[index_1][index_2] = (s0 * s1 * c2) - (c0 * s2); - (*this)[2][0] = -s1; - (*this)[2][1] = c1 * s2; - (*this)[2][2] = c1 * c2; + (*this)[index_2][index_0] = -s1; + (*this)[index_2][index_1] = c1 * s2; + (*this)[index_2][index_2] = c1 * c2; return *this; } @@ -931,17 +953,20 @@ vpRotationMatrix &vpRotationMatrix::build(const vpQuaternionVector &q) double b = q.x(); double c = q.y(); double d = q.z(); - (*this)[0][0] = (((a * a) + (b * b)) - (c * c)) - (d * d); - (*this)[0][1] = (2 * b * c) - (2 * a * d); - (*this)[0][2] = (2 * a * c) + (2 * b * d); - - (*this)[1][0] = (2 * a * d) + (2 * b * c); - (*this)[1][1] = (((a * a) - (b * b)) + (c * c)) - (d * d); - (*this)[1][2] = (2 * c * d) - (2 * a * b); - - (*this)[2][0] = (2 * b * d) - (2 * a * c); - (*this)[2][1] = (2 * a * b) + (2 * c * d); - (*this)[2][2] = ((a * a) - (b * b) - (c * c)) + (d * d); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + (*this)[index_0][index_0] = (((a * a) + (b * b)) - (c * c)) - (d * d); + (*this)[index_0][index_1] = (2 * b * c) - (2 * a * d); + (*this)[index_0][index_2] = (2 * a * c) + (2 * b * d); + + (*this)[index_1][index_0] = (2 * a * d) + (2 * b * c); + (*this)[index_1][index_1] = (((a * a) - (b * b)) + (c * c)) - (d * d); + (*this)[index_1][index_2] = (2 * c * d) - (2 * a * b); + + (*this)[index_2][index_0] = (2 * b * d) - (2 * a * c); + (*this)[index_2][index_1] = (2 * a * b) + (2 * c * d); + (*this)[index_2][index_2] = ((a * a) - (b * b) - (c * c)) + (d * d); return *this; } @@ -1020,16 +1045,19 @@ vpRotationMatrix vpRotationMatrix::mean(const std::vector & vpMatrix M, U, V; vpColVector sv; meanR.pseudoInverse(M, sv, 1e-6, U, V); - double det = sv[0] * sv[1] * sv[2]; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + double det = sv[index_0] * sv[index_1] * sv[index_2]; if (det > 0) { meanR = U * V.t(); } else { vpMatrix D(3, 3); D = 0.0; - D[0][0] = 1.0; - D[1][1] = 1.0; - D[2][2] = -1; + D[index_0][index_0] = 1.0; + D[index_1][index_1] = 1.0; + D[index_2][index_2] = -1; meanR = U * D * V.t(); } @@ -1059,16 +1087,19 @@ vpRotationMatrix vpRotationMatrix::mean(const std::vector &vec vpMatrix M, U, V; vpColVector sv; meanR.pseudoInverse(M, sv, 1e-6, U, V); - double det = sv[0] * sv[1] * sv[2]; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + double det = sv[index_0] * sv[index_1] * sv[index_2]; if (det > 0) { meanR = U * V.t(); } else { vpMatrix D(3, 3); D = 0.0; - D[0][0] = 1.0; - D[1][1] = 1.0; - D[2][2] = -1; + D[index_0][index_0] = 1.0; + D[index_1][index_1] = 1.0; + D[index_2][index_2] = -1; meanR = U * D * V.t(); } @@ -1087,25 +1118,34 @@ void vpRotationMatrix::orthogonalize() U.svd(w, V); vpMatrix Vt = V.t(); vpMatrix R = U * Vt; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + const unsigned int index_5 = 5; + const unsigned int index_6 = 6; + const unsigned int index_7 = 7; + const unsigned int index_8 = 8; double det = R.det(); if (det < 0) { - Vt[2][0] *= -1; - Vt[2][1] *= -1; - Vt[2][2] *= -1; + Vt[index_2][index_0] *= -1; + Vt[index_2][index_1] *= -1; + Vt[index_2][index_2] *= -1; R = U * Vt; } - data[0] = R[0][0]; - data[1] = R[0][1]; - data[2] = R[0][2]; - data[3] = R[1][0]; - data[4] = R[1][1]; - data[5] = R[1][2]; - data[6] = R[2][0]; - data[7] = R[2][1]; - data[8] = R[2][2]; + data[index_0] = R[index_0][index_0]; + data[index_1] = R[index_0][index_1]; + data[index_2] = R[index_0][index_2]; + data[index_3] = R[index_1][index_0]; + data[index_4] = R[index_1][index_1]; + data[index_5] = R[index_1][index_2]; + data[index_6] = R[index_2][index_0]; + data[index_7] = R[index_2][index_1]; + data[index_8] = R[index_2][index_2]; } #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) diff --git a/modules/core/src/math/transformation/vpRotationVector.cpp b/modules/core/src/math/transformation/vpRotationVector.cpp index 752a9e5e6c..f14f901966 100644 --- a/modules/core/src/math/transformation/vpRotationVector.cpp +++ b/modules/core/src/math/transformation/vpRotationVector.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/core/src/math/transformation/vpRxyzVector.cpp b/modules/core/src/math/transformation/vpRxyzVector.cpp index 1d8ffc3d82..782f688fe2 100644 --- a/modules/core/src/math/transformation/vpRxyzVector.cpp +++ b/modules/core/src/math/transformation/vpRxyzVector.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -151,18 +150,21 @@ vpRxyzVector &vpRxyzVector::build(const vpRotationMatrix &R) { double COEF_MIN_ROT = 1e-6; double phi; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; - if ((fabs(R[1][2]) < COEF_MIN_ROT) && (fabs(R[2][2]) < COEF_MIN_ROT)) { + if ((fabs(R[index_1][index_2]) < COEF_MIN_ROT) && (fabs(R[index_2][index_2]) < COEF_MIN_ROT)) { phi = 0; } else { - phi = atan2(-R[1][2], R[2][2]); + phi = atan2(-R[index_1][index_2], R[index_2][index_2]); } double si = sin(phi); double co = cos(phi); - double theta = atan2(R[0][2], (-si * R[1][2]) + (co * R[2][2])); - double psi = atan2((co * R[1][0]) + (si * R[2][0]), (co * R[1][1]) + (si * R[2][1])); + double theta = atan2(R[index_0][index_2], (-si * R[index_1][index_2]) + (co * R[index_2][index_2])); + double psi = atan2((co * R[index_1][index_0]) + (si * R[index_2][index_0]), (co * R[index_1][index_1]) + (si * R[index_2][index_1])); build(phi, theta, psi); @@ -192,9 +194,12 @@ vpRxyzVector &vpRxyzVector::build(const vpThetaUVector &tu) */ vpRxyzVector &vpRxyzVector::build(const double &phi, const double &theta, const double &psi) { - data[0] = phi; - data[1] = theta; - data[2] = psi; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + data[index_0] = phi; + data[index_1] = theta; + data[index_2] = psi; return *this; } @@ -207,7 +212,8 @@ vpRxyzVector &vpRxyzVector::build(const vpColVector &rxyz) throw(vpException(vpException::dimensionError, "Cannot construct a R-xyz vector from a %d-dimension col vector", rxyz.size())); } - for (unsigned int i = 0; i < 3; ++i) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { data[i] = rxyz[i]; } @@ -223,7 +229,8 @@ vpRxyzVector &vpRxyzVector::build(const std::vector &rxyz) throw(vpException(vpException::dimensionError, "Cannot construct a R-xyz vector from a %d-dimension std::vector", rxyz.size())); } - for (unsigned int i = 0; i < 3; ++i) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { data[i] = rxyz[i]; } @@ -286,7 +293,8 @@ vpRxyzVector &vpRxyzVector::operator=(const vpColVector &rxyz) throw(vpException(vpException::dimensionError, "Cannot set a R-xyz vector from a %d-dimension col vector", rxyz.size())); } - for (unsigned int i = 0; i < 3; ++i) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { data[i] = rxyz[i]; } diff --git a/modules/core/src/math/transformation/vpRzyxVector.cpp b/modules/core/src/math/transformation/vpRzyxVector.cpp index 47732cb5e6..84182f0de5 100644 --- a/modules/core/src/math/transformation/vpRzyxVector.cpp +++ b/modules/core/src/math/transformation/vpRzyxVector.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -156,8 +155,11 @@ vpRzyxVector vpRzyxVector::buildFrom(const std::vector &rzyx) */ vpRzyxVector &vpRzyxVector::build(const vpRotationMatrix &R) { - double nx = R[0][0]; - double ny = R[1][0]; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + double nx = R[index_0][index_0]; + double ny = R[index_1][index_0]; double COEF_MIN_ROT = 1e-6; double phi; @@ -171,13 +173,13 @@ vpRzyxVector &vpRzyxVector::build(const vpRotationMatrix &R) double si = sin(phi); double co = cos(phi); - double nz = R[2][0]; + double nz = R[index_2][index_0]; double theta = atan2(-nz, (co * nx) + (si * ny)); - double ax = R[0][2]; - double ay = R[1][2]; - double ox = R[0][1]; - double oy = R[1][1]; + double ax = R[index_0][index_2]; + double ay = R[index_1][index_2]; + double ox = R[index_0][index_1]; + double oy = R[index_1][index_1]; double psi = atan2((si * ax) - (co * ay), (-si * ox) + (co * oy)); @@ -209,9 +211,12 @@ vpRzyxVector &vpRzyxVector::build(const vpThetaUVector &tu) */ vpRzyxVector &vpRzyxVector::build(const double &phi, const double &theta, const double &psi) { - data[0] = phi; - data[1] = theta; - data[2] = psi; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + data[index_0] = phi; + data[index_1] = theta; + data[index_2] = psi; return *this; } @@ -224,7 +229,8 @@ vpRzyxVector &vpRzyxVector::build(const vpColVector &rzyx) throw(vpException(vpException::dimensionError, "Cannot construct a R-zyx vector from a %d-dimension col vector", rzyx.size())); } - for (unsigned int i = 0; i < 3; ++i) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { data[i] = rzyx[i]; } @@ -240,7 +246,8 @@ vpRzyxVector &vpRzyxVector::build(const std::vector &rzyx) throw(vpException(vpException::dimensionError, "Cannot construct a R-zyx vector from a %d-dimension std::vector", rzyx.size())); } - for (unsigned int i = 0; i < 3; ++i) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { data[i] = rzyx[i]; } @@ -303,7 +310,8 @@ vpRzyxVector &vpRzyxVector::operator=(const vpColVector &rzyx) throw(vpException(vpException::dimensionError, "Cannot set a R-zyx vector from a %d-dimension col vector", rzyx.size())); } - for (unsigned int i = 0; i < 3; ++i) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { data[i] = rzyx[i]; } diff --git a/modules/core/src/math/transformation/vpRzyzVector.cpp b/modules/core/src/math/transformation/vpRzyzVector.cpp index 154ca166c1..8987d98b8c 100644 --- a/modules/core/src/math/transformation/vpRzyzVector.cpp +++ b/modules/core/src/math/transformation/vpRzyzVector.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -148,16 +147,19 @@ void vpRzyzVector::buildFrom(double phi, double theta, double psi) vpRzyzVector &vpRzyzVector::build(const vpRotationMatrix &R) { double phi; - if ((fabs(R[1][2]) < 1e-6) && (fabs(R[0][2]) < 1e-6)) { + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + if ((fabs(R[index_1][index_2]) < 1e-6) && (fabs(R[index_0][index_2]) < 1e-6)) { phi = 0; } else { - phi = atan2(R[1][2], R[0][2]); + phi = atan2(R[index_1][index_2], R[index_0][index_2]); } double cphi = cos(phi); double sphi = sin(phi); - double theta = atan2((cphi * R[0][2]) + (sphi * R[1][2]), R[2][2]); + double theta = atan2((cphi * R[index_0][index_2]) + (sphi * R[index_1][index_2]), R[index_2][index_2]); double psi = atan2((-sphi * R[0][0]) + (cphi * R[1][0]), (-sphi * R[0][1]) + (cphi * R[1][1])); @@ -190,7 +192,8 @@ vpRzyzVector &vpRzyzVector::build(const vpColVector &rzyz) throw(vpException(vpException::dimensionError, "Cannot construct a R-zyz vector from a %d-dimension col vector", rzyz.size())); } - for (unsigned int i = 0; i < 3; ++i) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { data[i] = rzyz[i]; } @@ -206,7 +209,8 @@ vpRzyzVector &vpRzyzVector::build(const std::vector &rzyz) throw(vpException(vpException::dimensionError, "Cannot construct a R-zyx vector from a %d-dimension std::vector", rzyz.size())); } - for (unsigned int i = 0; i < 3; ++i) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { data[i] = rzyz[i]; } @@ -221,9 +225,12 @@ vpRzyzVector &vpRzyzVector::build(const std::vector &rzyz) */ vpRzyzVector &vpRzyzVector::build(const double &phi, const double &theta, const double &psi) { - data[0] = phi; - data[1] = theta; - data[2] = psi; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + data[index_0] = phi; + data[index_1] = theta; + data[index_2] = psi; return *this; } @@ -283,7 +290,8 @@ vpRzyzVector &vpRzyzVector::operator=(const vpColVector &rzyz) throw(vpException(vpException::dimensionError, "Cannot set a R-zyz vector from a %d-dimension col vector", rzyz.size())); } - for (unsigned int i = 0; i < 3; ++i) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { data[i] = rzyz[i]; } diff --git a/modules/core/src/math/transformation/vpThetaUVector.cpp b/modules/core/src/math/transformation/vpThetaUVector.cpp index 7f62f8cae8..7b1d664eb4 100644 --- a/modules/core/src/math/transformation/vpThetaUVector.cpp +++ b/modules/core/src/math/transformation/vpThetaUVector.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -223,7 +222,8 @@ vpThetaUVector &vpThetaUVector::build(const vpHomogeneousMatrix &M) */ vpThetaUVector &vpThetaUVector::build(const vpPoseVector &p) { - for (unsigned int i = 0; i < 3; ++i) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { data[i] = p[i + 3]; } @@ -236,11 +236,14 @@ vpThetaUVector &vpThetaUVector::build(const vpPoseVector &p) vpThetaUVector &vpThetaUVector::build(const vpRotationMatrix &R) { double s, c, theta; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; - s = ((R[1][0] - R[0][1]) * (R[1][0] - R[0][1])) + ((R[2][0] - R[0][2]) * (R[2][0] - R[0][2])) + - ((R[2][1] - R[1][2]) * (R[2][1] - R[1][2])); + s = ((R[1][0] - R[0][1]) * (R[1][0] - R[0][1])) + ((R[index_2][0] - R[0][index_2]) * (R[index_2][0] - R[0][index_2])) + + ((R[index_2][index_1] - R[index_1][index_2]) * (R[index_2][index_1] - R[index_1][index_2])); s = sqrt(s) / 2.0; - c = ((R[0][0] + R[1][1] + R[2][2]) - 1.0) / 2.0; + c = ((R[index_0][index_0] + R[index_1][index_1] + R[index_2][index_2]) - 1.0) / 2.0; theta = atan2(s, c); /* theta in [0, PI] since s > 0 */ // General case when theta != pi. If theta=pi, c=-1 @@ -248,9 +251,9 @@ vpThetaUVector &vpThetaUVector::build(const vpRotationMatrix &R) { double sinc = vpMath::sinc(s, theta); - data[0] = (R[2][1] - R[1][2]) / (2 * sinc); - data[1] = (R[0][2] - R[2][0]) / (2 * sinc); - data[2] = (R[1][0] - R[0][1]) / (2 * sinc); + data[index_0] = (R[index_2][index_1] - R[index_1][index_2]) / (2 * sinc); + data[index_1] = (R[index_0][index_2] - R[index_2][index_0]) / (2 * sinc); + data[index_2] = (R[index_1][index_0] - R[index_0][index_1]) / (2 * sinc); } else /* theta near PI */ { @@ -265,29 +268,29 @@ vpThetaUVector &vpThetaUVector::build(const vpRotationMatrix &R) } double z = 0; - if ((R[2][2] - c) > std::numeric_limits::epsilon()) { - z = sqrt((R[2][2] - c) / (1 - c)); + if ((R[index_2][index_2] - c) > std::numeric_limits::epsilon()) { + z = sqrt((R[index_2][index_2] - c) / (1 - c)); } if ((x > y) && (x > z)) { - if ((R[2][1] - R[1][2]) < 0) { + if ((R[index_2][index_1] - R[index_1][index_2]) < 0) { x = -x; } if ((vpMath::sign(x) * vpMath::sign(y)) != (vpMath::sign(R[0][1] + R[1][0]))) { y = -y; } - if ((vpMath::sign(x) * vpMath::sign(z)) != (vpMath::sign(R[0][2] + R[2][0]))) { + if ((vpMath::sign(x) * vpMath::sign(z)) != (vpMath::sign(R[index_0][index_2] + R[index_2][index_0]))) { z = -z; } } else if (y > z) { - if ((R[0][2] - R[2][0]) < 0) { + if ((R[index_0][index_2] - R[index_2][index_0]) < 0) { y = -y; } - if ((vpMath::sign(y) * vpMath::sign(x)) != (vpMath::sign(R[1][0] + R[0][1]))) { + if ((vpMath::sign(y) * vpMath::sign(x)) != (vpMath::sign(R[index_1][index_0] + R[index_0][index_1]))) { x = -x; } - if ((vpMath::sign(y) * vpMath::sign(z)) != (vpMath::sign(R[1][2] + R[2][1]))) { + if ((vpMath::sign(y) * vpMath::sign(z)) != (vpMath::sign(R[index_1][index_2] + R[index_2][index_1]))) { z = -z; } } @@ -295,16 +298,16 @@ vpThetaUVector &vpThetaUVector::build(const vpRotationMatrix &R) if ((R[1][0] - R[0][1]) < 0) { z = -z; } - if ((vpMath::sign(z) * vpMath::sign(x)) != (vpMath::sign(R[2][0] + R[0][2]))) { + if ((vpMath::sign(z) * vpMath::sign(x)) != (vpMath::sign(R[index_2][index_0] + R[index_0][index_2]))) { x = -x; } - if ((vpMath::sign(z) * vpMath::sign(y)) != (vpMath::sign(R[2][1] + R[1][2]))) { + if ((vpMath::sign(z) * vpMath::sign(y)) != (vpMath::sign(R[index_2][index_1] + R[index_1][index_2]))) { y = -y; } } - data[0] = theta * x; - data[1] = theta * y; - data[2] = theta * z; + data[index_0] = theta * x; + data[index_1] = theta * y; + data[index_2] = theta * z; } return *this; @@ -360,7 +363,8 @@ vpThetaUVector &vpThetaUVector::build(const std::vector &tu) throw(vpException(vpException::dimensionError, "Cannot construct a theta-u vector from a %d-dimension std::vector", tu.size())); } - for (unsigned int i = 0; i < 3; ++i) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { data[i] = tu[i]; } @@ -376,7 +380,8 @@ vpThetaUVector &vpThetaUVector::build(const vpColVector &tu) throw(vpException(vpException::dimensionError, "Cannot construct a theta-u vector from a %d-dimension std::vector", tu.size())); } - for (unsigned int i = 0; i < 3; ++i) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { data[i] = tu[i]; } @@ -388,9 +393,12 @@ vpThetaUVector &vpThetaUVector::build(const vpColVector &tu) */ vpThetaUVector &vpThetaUVector::build(const double &tux, const double &tuy, const double &tuz) { - data[0] = tux; - data[1] = tuy; - data[2] = tuz; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + data[index_0] = tux; + data[index_1] = tuy; + data[index_2] = tuz; return *this; } @@ -498,7 +506,8 @@ void vpThetaUVector::extract(double &theta, vpColVector &u) const u = 0; return; } - for (unsigned int i = 0; i < 3; ++i) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { u[i] = data[i] / theta; } } @@ -525,7 +534,13 @@ void vpThetaUVector::extract(double &theta, vpColVector &u) const \sa getTheta(), extract() */ -double vpThetaUVector::getTheta() const { return sqrt((data[0] * data[0]) + (data[1] * data[1]) + (data[2] * data[2])); } +double vpThetaUVector::getTheta() const +{ + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + return sqrt((data[index_0] * data[index_0]) + (data[index_1] * data[index_1]) + (data[index_2] * data[index_2])); +} /*! @@ -560,7 +575,8 @@ vpColVector vpThetaUVector::getU() const u = 0; return u; } - for (unsigned int i = 0; i < 3; ++i) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { u[i] = data[i] / theta; } return u; diff --git a/modules/core/src/math/transformation/vpTranslationVector.cpp b/modules/core/src/math/transformation/vpTranslationVector.cpp index b1d0a65ada..0dc49913d5 100644 --- a/modules/core/src/math/transformation/vpTranslationVector.cpp +++ b/modules/core/src/math/transformation/vpTranslationVector.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -52,9 +51,12 @@ BEGIN_VISP_NAMESPACE */ vpTranslationVector::vpTranslationVector(double tx, double ty, double tz) : vpArray2D(3, 1), m_index(0) { - (*this)[0] = tx; - (*this)[1] = ty; - (*this)[2] = tz; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + (*this)[index_0] = tx; + (*this)[index_1] = ty; + (*this)[index_2] = tz; } /*! @@ -78,9 +80,12 @@ vpTranslationVector::vpTranslationVector(const vpHomogeneousMatrix &M) : vpArray */ vpTranslationVector::vpTranslationVector(const vpPoseVector &p) : vpArray2D(3, 1), m_index(0) { - (*this)[0] = p[0]; - (*this)[1] = p[1]; - (*this)[2] = p[2]; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + (*this)[index_0] = p[index_0]; + (*this)[index_1] = p[index_1]; + (*this)[index_2] = p[index_2]; } /*! @@ -213,9 +218,12 @@ vpTranslationVector &vpTranslationVector::build(const vpHomogeneousMatrix &M) */ vpTranslationVector &vpTranslationVector::build(const vpPoseVector &p) { - (*this)[0] = p[0]; - (*this)[1] = p[1]; - (*this)[2] = p[2]; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + (*this)[index_0] = p[index_0]; + (*this)[index_1] = p[index_1]; + (*this)[index_2] = p[index_2]; return *this; } @@ -230,14 +238,18 @@ vpTranslationVector &vpTranslationVector::build(const vpPoseVector &p) */ vpTranslationVector &vpTranslationVector::build(const vpColVector &v) { - if (v.size() != 3) { + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int v_size = 3; + if (v.size() != v_size) { throw(vpException(vpException::dimensionError, "Cannot build a translation vector from a %d-dimension column vector", v.size())); } - (*this)[0] = v[0]; - (*this)[1] = v[1]; - (*this)[2] = v[2]; + (*this)[index_0] = v[index_0]; + (*this)[index_1] = v[index_1]; + (*this)[index_2] = v[index_2]; return *this; } @@ -263,9 +275,12 @@ vpTranslationVector &vpTranslationVector::build(const double &tx, const double & */ void vpTranslationVector::set(double tx, double ty, double tz) { - (*this)[0] = tx; - (*this)[1] = ty; - (*this)[2] = tz; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + (*this)[index_0] = tx; + (*this)[index_1] = ty; + (*this)[index_2] = tz; } /*! @@ -290,7 +305,8 @@ vpTranslationVector vpTranslationVector::operator+(const vpTranslationVector &tv { vpTranslationVector s; - for (unsigned int i = 0; i < 3; ++i) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { s[i] = (*this)[i] + tv[i]; } @@ -325,7 +341,8 @@ vpTranslationVector vpTranslationVector::operator+(const vpColVector &v) const } vpTranslationVector s; - for (unsigned int i = 0; i < 3; ++i) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { s[i] = (*this)[i] + v[i]; } @@ -354,7 +371,8 @@ vpTranslationVector vpTranslationVector::operator-(const vpTranslationVector &tv { vpTranslationVector sub; - for (unsigned int i = 0; i < 3; ++i) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { sub[i] = (*this)[i] - tv[i]; } @@ -695,16 +713,19 @@ vpTranslationVector &vpTranslationVector::operator,(double val) */ void vpTranslationVector::skew(const vpTranslationVector &tv, vpMatrix &M) { + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; M.resize(3, 3); - M[0][0] = 0; - M[0][1] = -tv[2]; - M[0][2] = tv[1]; - M[1][0] = tv[2]; - M[1][1] = 0; - M[1][2] = -tv[0]; - M[2][0] = -tv[1]; - M[2][1] = tv[0]; - M[2][2] = 0; + M[index_0][index_0] = 0; + M[index_0][index_1] = -tv[index_2]; + M[index_0][index_2] = tv[index_1]; + M[index_1][index_0] = tv[index_2]; + M[index_1][index_1] = 0; + M[index_1][index_2] = -tv[index_0]; + M[index_2][index_0] = -tv[index_1]; + M[index_2][index_1] = tv[index_0]; + M[index_2][index_2] = 0; } /*! diff --git a/modules/core/src/math/transformation/vpVelocityTwistMatrix.cpp b/modules/core/src/math/transformation/vpVelocityTwistMatrix.cpp index 876bde89ec..16b5e5086c 100644 --- a/modules/core/src/math/transformation/vpVelocityTwistMatrix.cpp +++ b/modules/core/src/math/transformation/vpVelocityTwistMatrix.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -69,8 +68,9 @@ vpVelocityTwistMatrix &vpVelocityTwistMatrix::operator=(const vpVelocityTwistMat */ void vpVelocityTwistMatrix::eye() { - for (unsigned int i = 0; i < 6; ++i) { - for (unsigned int j = 0; j < 6; ++j) { + const unsigned int nparam = 6; + for (unsigned int i = 0; i < nparam; ++i) { + for (unsigned int j = 0; j < nparam; ++j) { if (i == j) { (*this)[i][j] = 1.0; } @@ -223,11 +223,11 @@ vpVelocityTwistMatrix::vpVelocityTwistMatrix(double tx, double ty, double tz, do vpVelocityTwistMatrix vpVelocityTwistMatrix::operator*(const vpVelocityTwistMatrix &V) const { vpVelocityTwistMatrix p; - - for (unsigned int i = 0; i < 6; ++i) { - for (unsigned int j = 0; j < 6; ++j) { + const unsigned int nparam = 6; + for (unsigned int i = 0; i < nparam; ++i) { + for (unsigned int j = 0; j < nparam; ++j) { double s = 0; - for (int k = 0; k < 6; ++k) { + for (unsigned int k = 0; k < nparam; ++k) { s += rowPtrs[i][k] * V.rowPtrs[k][j]; } p[i][j] = s; @@ -277,17 +277,18 @@ matrix. */ vpMatrix vpVelocityTwistMatrix::operator*(const vpMatrix &M) const { - if (6 != M.getRows()) { + const unsigned int nparam = 6; + if (nparam != M.getRows()) { throw(vpException(vpException::dimensionError, "Cannot multiply a (6x6) velocity twist matrix by a (%dx%d) matrix", M.getRows(), M.getCols())); } - vpMatrix p(6, M.getCols()); + vpMatrix p(nparam, M.getCols()); unsigned int m_col = M.getCols(); - for (unsigned int i = 0; i < 6; ++i) { + for (unsigned int i = 0; i < nparam; ++i) { for (unsigned int j = 0; j < m_col; ++j) { double s = 0; - for (unsigned int k = 0; k < 6; ++k) { + for (unsigned int k = 0; k < nparam; ++k) { s += rowPtrs[i][k] * M[k][j]; } p[i][j] = s; @@ -309,9 +310,10 @@ vpMatrix vpVelocityTwistMatrix::operator*(const vpMatrix &M) const */ vpColVector vpVelocityTwistMatrix::operator*(const vpColVector &v) const { - vpColVector c(6); + const unsigned int nparam = 6; + vpColVector c(nparam); - if (6 != v.getRows()) { + if (nparam != v.getRows()) { throw(vpException(vpException::dimensionError, "Cannot multiply a (6x6) velocity twist matrix by a " "(%d) column vector", @@ -320,8 +322,8 @@ vpColVector vpVelocityTwistMatrix::operator*(const vpColVector &v) const c = 0.0; - for (unsigned int i = 0; i < 6; ++i) { - for (unsigned int j = 0; j < 6; ++j) { + for (unsigned int i = 0; i < nparam; ++i) { + for (unsigned int j = 0; j < nparam; ++j) { c[i] += rowPtrs[i][j] * v[j]; } } @@ -444,8 +446,9 @@ vpVelocityTwistMatrix vpVelocityTwistMatrix::buildFrom(const vpHomogeneousMatrix */ vpVelocityTwistMatrix &vpVelocityTwistMatrix::build(const vpRotationMatrix &R) { - for (unsigned int i = 0; i < 3; ++i) { - for (unsigned int j = 0; j < 3; ++j) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { + for (unsigned int j = 0; j < val_3; ++j) { (*this)[i][j] = R[i][j]; (*this)[i + 3][j + 3] = R[i][j]; (*this)[i][j + 3] = 0; @@ -472,8 +475,9 @@ vpVelocityTwistMatrix &vpVelocityTwistMatrix::build(const vpTranslationVector &t { vpMatrix skewaR = t.skew(t) * R; - for (unsigned int i = 0; i < 3; ++i) { - for (unsigned int j = 0; j < 3; ++j) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { + for (unsigned int j = 0; j < val_3; ++j) { (*this)[i][j] = R[i][j]; (*this)[i + 3][j + 3] = R[i][j]; (*this)[i][j + 3] = skewaR[i][j]; @@ -574,8 +578,9 @@ void vpVelocityTwistMatrix::inverse(vpVelocityTwistMatrix &V) const { V = invers //! Extract the rotation matrix from the velocity twist matrix. void vpVelocityTwistMatrix::extract(vpRotationMatrix &R) const { - for (unsigned int i = 0; i < 3; ++i) { - for (unsigned int j = 0; j < 3; ++j) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { + for (unsigned int j = 0; j < val_3; ++j) { R[i][j] = (*this)[i][j]; } } @@ -587,16 +592,20 @@ void vpVelocityTwistMatrix::extract(vpTranslationVector &tv) const vpRotationMatrix R; extract(R); vpMatrix skTR(3, 3); - for (unsigned int i = 0; i < 3; ++i) { - for (unsigned int j = 0; j < 3; ++j) { - skTR[i][j] = (*this)[i][j + 3]; + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { + for (unsigned int j = 0; j < val_3; ++j) { + skTR[i][j] = (*this)[i][j + val_3]; } } vpMatrix skT = skTR * R.t(); - tv[0] = skT[2][1]; - tv[1] = skT[0][2]; - tv[2] = skT[1][0]; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + tv[index_0] = skT[index_2][index_1]; + tv[index_1] = skT[index_0][index_2]; + tv[index_2] = skT[index_1][index_0]; } /*! diff --git a/modules/core/src/math/transformation/vpXmlParserHomogeneousMatrix.cpp b/modules/core/src/math/transformation/vpXmlParserHomogeneousMatrix.cpp index 042cb63baa..49798c4440 100644 --- a/modules/core/src/math/transformation/vpXmlParserHomogeneousMatrix.cpp +++ b/modules/core/src/math/transformation/vpXmlParserHomogeneousMatrix.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,7 @@ * * Description: * XML parser to load and save Homogeneous Matrix in a XML file - * - * Authors: - * Giovanni Claudio - * -*****************************************************************************/ + */ /*! \file vpXmlParserHomogeneousMatrix.cpp @@ -124,21 +119,20 @@ class vpXmlParserHomogeneousMatrix::Impl unsigned int nbM = 0; for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) { - if (node.type() != pugi::node_element) - continue; - - if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) { - prop = CODE_XML_OTHER; - back = SEQUENCE_ERROR; - } + if (node.type() == pugi::node_element) { + if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) { + prop = CODE_XML_OTHER; + back = SEQUENCE_ERROR; + } - if (prop == CODE_XML_M) { - if (SEQUENCE_OK == read_matrix(node, name)) { - nbM++; + if (prop == CODE_XML_M) { + if (SEQUENCE_OK == read_matrix(node, name)) { + nbM++; + } + } + else { + back = SEQUENCE_ERROR; } - } - else { - back = SEQUENCE_ERROR; } } @@ -174,40 +168,39 @@ class vpXmlParserHomogeneousMatrix::Impl vpXmlCodeSequenceType back = SEQUENCE_OK; for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) { - if (node.type() != pugi::node_element) - continue; - - if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) { - prop = CODE_XML_OTHER; - back = SEQUENCE_ERROR; - } + if (node.type() == pugi::node_element) { + if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) { + prop = CODE_XML_OTHER; + back = SEQUENCE_ERROR; + } - switch (prop) { - case CODE_XML_M_NAME: { - M_name_tmp = node.text().as_string(); - break; - } + switch (prop) { + case CODE_XML_M_NAME: { + M_name_tmp = node.text().as_string(); + break; + } - case CODE_XML_VALUE: // VALUE - if (name == M_name_tmp) { - std::cout << "Found Homogeneous Matrix with name: \"" << M_name_tmp << "\"" << std::endl; - back = read_values(node, M_tmp); + case CODE_XML_VALUE: // VALUE + if (name == M_name_tmp) { + std::cout << "Found Homogeneous Matrix with name: \"" << M_name_tmp << "\"" << std::endl; + back = read_values(node, M_tmp); + } + break; + + case CODE_XML_BAD: + case CODE_XML_OTHER: + case CODE_XML_M: + case CODE_XML_TX: + case CODE_XML_TY: + case CODE_XML_TZ: + case CODE_XML_TUX: + case CODE_XML_TUY: + case CODE_XML_TUZ: + + default: + back = SEQUENCE_ERROR; + break; } - break; - - case CODE_XML_BAD: - case CODE_XML_OTHER: - case CODE_XML_M: - case CODE_XML_TX: - case CODE_XML_TY: - case CODE_XML_TZ: - case CODE_XML_TUX: - case CODE_XML_TUY: - case CODE_XML_TUZ: - - default: - back = SEQUENCE_ERROR; - break; } } @@ -248,49 +241,48 @@ class vpXmlParserHomogeneousMatrix::Impl vpXmlCodeSequenceType back = SEQUENCE_OK; for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) { - if (node.type() != pugi::node_element) - continue; - - if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) { - prop = CODE_XML_OTHER; - back = SEQUENCE_ERROR; - } + if (node.type() == pugi::node_element) { + if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) { + prop = CODE_XML_OTHER; + back = SEQUENCE_ERROR; + } - switch (prop) { - case CODE_XML_TX: - tx_ = node.text().as_double(); - nb++; - break; - case CODE_XML_TY: - ty_ = node.text().as_double(); - nb++; - break; - case CODE_XML_TZ: - tz_ = node.text().as_double(); - nb++; - break; - case CODE_XML_TUX: - tux_ = node.text().as_double(); - nb++; - break; - case CODE_XML_TUY: - tuy_ = node.text().as_double(); - nb++; - break; - case CODE_XML_TUZ: - tuz_ = node.text().as_double(); - nb++; - break; - - case CODE_XML_BAD: - case CODE_XML_OTHER: - case CODE_XML_M: - case CODE_XML_M_NAME: - case CODE_XML_VALUE: - - default: - back = SEQUENCE_ERROR; - break; + switch (prop) { + case CODE_XML_TX: + tx_ = node.text().as_double(); + nb++; + break; + case CODE_XML_TY: + ty_ = node.text().as_double(); + nb++; + break; + case CODE_XML_TZ: + tz_ = node.text().as_double(); + nb++; + break; + case CODE_XML_TUX: + tux_ = node.text().as_double(); + nb++; + break; + case CODE_XML_TUY: + tuy_ = node.text().as_double(); + nb++; + break; + case CODE_XML_TUZ: + tuz_ = node.text().as_double(); + nb++; + break; + + case CODE_XML_BAD: + case CODE_XML_OTHER: + case CODE_XML_M: + case CODE_XML_M_NAME: + case CODE_XML_VALUE: + + default: + back = SEQUENCE_ERROR; + break; + } } } @@ -371,15 +363,14 @@ class vpXmlParserHomogeneousMatrix::Impl int nbM = 0; for (pugi::xml_node node = node_.first_child(); node; node = node.next_sibling()) { - if (node.type() != pugi::node_element) - continue; - - if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) { - prop = CODE_XML_OTHER; - } - if (prop == CODE_XML_M) { - if (SEQUENCE_OK == read_matrix(node, name)) { - nbM++; + if (node.type() == pugi::node_element) { + if (SEQUENCE_OK != str2xmlcode(node.name(), prop)) { + prop = CODE_XML_OTHER; + } + if (prop == CODE_XML_M) { + if (SEQUENCE_OK == read_matrix(node, name)) { + nbM++; + } } } } diff --git a/modules/core/src/tools/cpu-features/vpCPUFeatures.cpp b/modules/core/src/tools/cpu-features/vpCPUFeatures.cpp index 2c32fb79bc..1cb6be9e37 100644 --- a/modules/core/src/tools/cpu-features/vpCPUFeatures.cpp +++ b/modules/core/src/tools/cpu-features/vpCPUFeatures.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/core/src/tools/cpu-features/x86/cpu_x86.cpp b/modules/core/src/tools/cpu-features/x86/cpu_x86.cpp index 274447c4d6..bcdfd083d1 100644 --- a/modules/core/src/tools/cpu-features/x86/cpu_x86.cpp +++ b/modules/core/src/tools/cpu-features/x86/cpu_x86.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -103,9 +103,12 @@ bool cpu_x86::detect_OS_AVX() uint32_t cpuInfo[4]; cpuid(cpuInfo, 1); + const unsigned int index_2 = 2; + const unsigned int val_27 = 27; + const unsigned int val_28 = 28; - bool osUsesXSAVE_XRSTORE = (cpuInfo[2] & (1U << 27)) != 0; - bool cpuAVXSuport = (cpuInfo[2] & (1U << 28)) != 0; + bool osUsesXSAVE_XRSTORE = (cpuInfo[index_2] & (1U << val_27)) != 0; + bool cpuAVXSuport = (cpuInfo[index_2] & (1U << val_28)) != 0; if (osUsesXSAVE_XRSTORE && cpuAVXSuport) { uint64_t xcrFeatureMask = xgetbv(_XCR_XFEATURE_ENABLED_MASK); @@ -137,9 +140,13 @@ std::string cpu_x86::get_vendor_string() char name[13]; cpuid(CPUInfo, 0); - memcpy(name + 0, &CPUInfo[1], 4); - memcpy(name + 4, &CPUInfo[3], 4); - memcpy(name + 8, &CPUInfo[2], 4); + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int val_4 = 4; + memcpy(name + 0, &CPUInfo[index_1], val_4); + memcpy(name + 4, &CPUInfo[index_3], val_4); + memcpy(name + 8, &CPUInfo[index_2], val_4); name[12] = '\0'; return name; @@ -174,53 +181,78 @@ void cpu_x86::detect_host() cpuid(info, 0x80000000); uint32_t nExIds = info[0]; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int val_1 = 1; + const unsigned int val_3 = 3; + const unsigned int val_5 = 5; + const unsigned int val_6 = 6; + const unsigned int val_8 = 8; + const unsigned int val_9 = 9; + const unsigned int val_11 = 11; + const unsigned int val_12 = 12; + const unsigned int val_14 = 14; + const unsigned int val_16 = 16; + const unsigned int val_17 = 17; + const unsigned int val_19 = 19; + const unsigned int val_20 = 20; + const unsigned int val_21 = 21; + const unsigned int val_23 = 23; + const unsigned int val_25 = 25; + const unsigned int val_26 = 26; + const unsigned int val_27 = 27; + const unsigned int val_28 = 28; + const unsigned int val_29 = 29; + const unsigned int val_30 = 30; + const unsigned int val_31 = 31; // Detect Features if (nIds >= 0x00000001) { cpuid(info, 0x00000001); - HW_MMX = (info[3] & (1U << 23)) != 0U; - HW_SSE = (info[3] & (1U << 25)) != 0U; - HW_SSE2 = (info[3] & (1U << 26)) != 0U; - HW_SSE3 = (info[2] & (1U << 0)) != 0U; + HW_MMX = (info[index_3] & (1U << val_23)) != 0U; + HW_SSE = (info[index_3] & (1U << val_25)) != 0U; + HW_SSE2 = (info[index_3] & (1U << val_26)) != 0U; + HW_SSE3 = (info[index_2] & (1U << 0)) != 0U; - HW_SSSE3 = (info[2] & (1U << 9)) != 0U; - HW_SSE41 = (info[2] & (1U << 19)) != 0U; - HW_SSE42 = (info[2] & (1U << 20)) != 0U; - HW_AES = (info[2] & (1U << 25)) != 0U; + HW_SSSE3 = (info[index_2] & (1U << val_9)) != 0U; + HW_SSE41 = (info[index_2] & (1U << val_19)) != 0U; + HW_SSE42 = (info[index_2] & (1U << val_20)) != 0U; + HW_AES = (info[index_2] & (1U << val_25)) != 0U; - HW_AVX = (info[2] & (1U << 28)) != 0U; - HW_FMA3 = (info[2] & (1U << 12)) != 0U; + HW_AVX = (info[index_2] & (1U << val_28)) != 0U; + HW_FMA3 = (info[index_2] & (1U << val_12)) != 0U; - HW_RDRAND = (info[2] & (1U << 30)) != 0U; + HW_RDRAND = (info[index_2] & (1U << val_30)) != 0U; } if (nIds >= 0x00000007) { cpuid(info, 0x00000007); - HW_AVX2 = (info[1] & (1U << 5)) != 0U; + HW_AVX2 = (info[index_1] & (1U << val_5)) != 0U; - HW_BMI1 = (info[1] & (1U << 3)) != 0U; - HW_BMI2 = (info[1] & (1U << 8)) != 0U; - HW_ADX = (info[1] & (1U << 19)) != 0U; - HW_MPX = (info[1] & (1U << 14)) != 0U; - HW_SHA = (info[1] & (1U << 29)) != 0U; - HW_PREFETCHWT1 = (info[2] & (1U << 0)) != 0U; + HW_BMI1 = (info[index_1] & (1U << val_3)) != 0U; + HW_BMI2 = (info[index_1] & (1U << val_8)) != 0U; + HW_ADX = (info[index_1] & (1U << val_19)) != 0U; + HW_MPX = (info[index_1] & (1U << val_14)) != 0U; + HW_SHA = (info[index_1] & (1U << val_29)) != 0U; + HW_PREFETCHWT1 = (info[index_2] & (1U << 0)) != 0U; - HW_AVX512_F = (info[1] & (1U << 16)) != 0U; - HW_AVX512_CD = (info[1] & (1U << 28)) != 0U; - HW_AVX512_PF = (info[1] & (1U << 26)) != 0U; - HW_AVX512_ER = (info[1] & (1U << 27)) != 0U; - HW_AVX512_VL = (info[1] & (1U << 31)) != 0U; - HW_AVX512_BW = (info[1] & (1U << 30)) != 0U; - HW_AVX512_DQ = (info[1] & (1U << 17)) != 0U; - HW_AVX512_IFMA = (info[1] & (1U << 21)) != 0U; - HW_AVX512_VBMI = (info[2] & (1U << 1)) != 0U; + HW_AVX512_F = (info[index_1] & (1U << val_16)) != 0U; + HW_AVX512_CD = (info[index_1] & (1U << val_28)) != 0U; + HW_AVX512_PF = (info[index_1] & (1U << val_26)) != 0U; + HW_AVX512_ER = (info[index_1] & (1U << val_27)) != 0U; + HW_AVX512_VL = (info[index_1] & (1U << val_31)) != 0U; + HW_AVX512_BW = (info[index_1] & (1U << val_30)) != 0U; + HW_AVX512_DQ = (info[index_1] & (1U << val_17)) != 0U; + HW_AVX512_IFMA = (info[index_1] & (1U << val_21)) != 0U; + HW_AVX512_VBMI = (info[index_2] & (1U << val_1)) != 0U; } if (nExIds >= 0x80000001) { cpuid(info, 0x80000001); - HW_x64 = (info[3] & (1U << 29)) != 0U; - HW_ABM = (info[2] & (1U << 5)) != 0U; - HW_SSE4a = (info[2] & (1U << 6)) != 0U; - HW_FMA4 = (info[2] & (1U << 16)) != 0U; - HW_XOP = (info[2] & (1U << 11)) != 0U; + HW_x64 = (info[index_3] & (1U << val_29)) != 0U; + HW_ABM = (info[index_2] & (1U << val_5)) != 0U; + HW_SSE4a = (info[index_2] & (1U << val_6)) != 0U; + HW_FMA4 = (info[index_2] & (1U << val_16)) != 0U; + HW_XOP = (info[index_2] & (1U << val_11)) != 0U; } #endif } diff --git a/modules/core/src/tools/cpu-features/x86/cpu_x86.h b/modules/core/src/tools/cpu-features/x86/cpu_x86.h index a9cf3ec49a..2559a9f331 100644 --- a/modules/core/src/tools/cpu-features/x86/cpu_x86.h +++ b/modules/core/src/tools/cpu-features/x86/cpu_x86.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -37,8 +37,8 @@ */ #pragma once -#ifndef _cpu_x86_H -#define _cpu_x86_H +#ifndef CPU_X86_H +#define CPU_X86_H //////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////// diff --git a/modules/core/src/tools/endian/vpEndian.cpp b/modules/core/src/tools/endian/vpEndian.cpp index ccc879ddde..c334d45234 100644 --- a/modules/core/src/tools/endian/vpEndian.cpp +++ b/modules/core/src/tools/endian/vpEndian.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -71,11 +70,15 @@ float swapFloat(float f) unsigned char b[4]; } dat1, dat2; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; dat1.f = f; - dat2.b[0] = dat1.b[3]; - dat2.b[1] = dat1.b[2]; - dat2.b[2] = dat1.b[1]; - dat2.b[3] = dat1.b[0]; + dat2.b[index_0] = dat1.b[index_3]; + dat2.b[index_1] = dat1.b[index_2]; + dat2.b[index_2] = dat1.b[index_1]; + dat2.b[index_3] = dat1.b[index_0]; return dat2.f; } @@ -91,15 +94,23 @@ double swapDouble(double d) unsigned char b[8]; } dat1, dat2; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + const unsigned int index_5 = 5; + const unsigned int index_6 = 6; + const unsigned int index_7 = 7; dat1.d = d; - dat2.b[0] = dat1.b[7]; - dat2.b[1] = dat1.b[6]; - dat2.b[2] = dat1.b[5]; - dat2.b[3] = dat1.b[4]; - dat2.b[4] = dat1.b[3]; - dat2.b[5] = dat1.b[2]; - dat2.b[6] = dat1.b[1]; - dat2.b[7] = dat1.b[0]; + dat2.b[index_0] = dat1.b[index_7]; + dat2.b[index_1] = dat1.b[index_6]; + dat2.b[index_2] = dat1.b[index_5]; + dat2.b[index_3] = dat1.b[index_4]; + dat2.b[index_4] = dat1.b[index_3]; + dat2.b[index_5] = dat1.b[index_2]; + dat2.b[index_6] = dat1.b[index_1]; + dat2.b[index_7] = dat1.b[index_0]; return dat2.d; } diff --git a/modules/core/src/tools/exceptions/vpException.cpp b/modules/core/src/tools/exceptions/vpException.cpp index a3d7859063..56b8135fbb 100644 --- a/modules/core/src/tools/exceptions/vpException.cpp +++ b/modules/core/src/tools/exceptions/vpException.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -56,8 +56,8 @@ vpException::vpException(int id, const char *format, va_list args) : code(id), m void vpException::setMessage(const char *format, va_list args) { - char buffer[1024]; - vsnprintf(buffer, 1024, format, args); + char buffer[FILENAME_MAX]; + vsnprintf(buffer, FILENAME_MAX, format, args); std::string msg(buffer); message = msg; } diff --git a/modules/core/src/tools/file/vpIoTools.cpp b/modules/core/src/tools/file/vpIoTools.cpp index 022edf9365..3781995b7c 100644 --- a/modules/core/src/tools/file/vpIoTools.cpp +++ b/modules/core/src/tools/file/vpIoTools.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -62,7 +61,6 @@ #include #include #include -#include #include #include #include @@ -349,20 +347,28 @@ visp::cnpy::npz_t visp::cnpy::npz_load(std::string fname) visp::cnpy::npz_t arrays; bool quit = false; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_26 = 26; + const unsigned int index_28 = 28; + const unsigned int val_8 = 8; + const unsigned int val_18 = 18; + const unsigned int val_22 = 22; + const unsigned int val_30 = 30; while (!quit) { std::vector local_header(30); - size_t headerres = fread(&local_header[0], sizeof(char), 30, fp); + size_t headerres = fread(&local_header[0], sizeof(char), val_30, fp); 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)) { + if ((local_header[index_2] != 0x03) || (local_header[index_3] != 0x04)) { quit = true; } else { //read in the variable name - uint16_t name_len = *(uint16_t *)&local_header[26]; + uint16_t name_len = *(uint16_t *)&local_header[index_26]; std::string varname(name_len, ' '); size_t vname_res = fread(&varname[0], sizeof(char), name_len, fp); if (vname_res != name_len) { @@ -373,7 +379,7 @@ visp::cnpy::npz_t visp::cnpy::npz_load(std::string fname) varname.erase(varname.end()-4, varname.end()); //read in the extra field - uint16_t extra_field_len = *(uint16_t *)&local_header[28]; + uint16_t extra_field_len = *(uint16_t *)&local_header[index_28]; if (extra_field_len > 0) { std::vector buff(extra_field_len); size_t efield_res = fread(&buff[0], sizeof(char), extra_field_len, fp); @@ -382,9 +388,9 @@ visp::cnpy::npz_t visp::cnpy::npz_load(std::string fname) } } - uint16_t compr_method = *reinterpret_cast(&local_header[0]+8); - uint32_t compr_bytes = *reinterpret_cast(&local_header[0]+18); - uint32_t uncompr_bytes = *reinterpret_cast(&local_header[0]+22); + uint16_t compr_method = *reinterpret_cast(&local_header[0] + val_8); + uint32_t compr_bytes = *reinterpret_cast(&local_header[0] + val_18); + uint32_t uncompr_bytes = *reinterpret_cast(&local_header[0] + val_22); if (compr_method == 0) { arrays[varname] = load_the_npy_file(fp); } else { arrays[varname] = load_the_npz_array(fp, compr_bytes, uncompr_bytes); } @@ -413,20 +419,28 @@ visp::cnpy::NpyArray visp::cnpy::npz_load(std::string fname, std::string varname } bool quit = false; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_26 = 26; + const unsigned int index_28 = 28; + const unsigned int val_8 = 8; + const unsigned int val_18 = 18; + const unsigned int val_22 = 22; + const unsigned int val_30 = 30; while (!quit) { std::vector local_header(30); - size_t header_res = fread(&local_header[0], sizeof(char), 30, fp); + size_t header_res = fread(&local_header[0], sizeof(char), val_30, fp); 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)) { + if ((local_header[index_2] != 0x03) || (local_header[index_3] != 0x04)) { quit = true; } else { //read in the variable name - uint16_t name_len = *(uint16_t *)&local_header[26]; + uint16_t name_len = *(uint16_t *)&local_header[index_26]; std::string vname(name_len, ' '); size_t vname_res = fread(&vname[0], sizeof(char), name_len, fp); if (vname_res != name_len) { @@ -435,12 +449,12 @@ visp::cnpy::NpyArray visp::cnpy::npz_load(std::string fname, std::string varname vname.erase(vname.end()-4, vname.end()); //erase the lagging .npy //read in the extra field - uint16_t extra_field_len = *(uint16_t *)&local_header[28]; + uint16_t extra_field_len = *(uint16_t *)&local_header[index_28]; fseek(fp, extra_field_len, SEEK_CUR); //skip past the extra field - uint16_t compr_method = *reinterpret_cast(&local_header[0]+8); - uint32_t compr_bytes = *reinterpret_cast(&local_header[0]+18); - uint32_t uncompr_bytes = *reinterpret_cast(&local_header[0]+22); + uint16_t compr_method = *reinterpret_cast(&local_header[0] + val_8); + uint32_t compr_bytes = *reinterpret_cast(&local_header[0] + val_18); + uint32_t uncompr_bytes = *reinterpret_cast(&local_header[0] + val_22); if (vname == varname) { NpyArray array = (compr_method == 0) ? load_the_npy_file(fp) : load_the_npz_array(fp, compr_bytes, uncompr_bytes); @@ -933,22 +947,25 @@ int vpIoTools::mkdir_p(const std::string &path, int mode) 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 + bool stop_for_loop = false; if (pos == 0) { cpy_path.erase(0, pos + 1); - continue; + stop_for_loop = true; } + if (!stop_for_loop) { #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) + (void)mode; // var not used + if (!checkDirectory(sub_path) && _mkdir(sub_path.c_str()) != 0) #endif - { - if (errno != EEXIST) { - return -1; + { + if (errno != EEXIST) { + return -1; + } } + cpy_path.erase(0, pos + 1); } - cpy_path.erase(0, pos + 1); } if (!cpy_path.empty()) { @@ -2134,16 +2151,16 @@ std::string vpIoTools::toLowerCase(const std::string &input) out += std::tolower(*it); } return out; -} + } -/** - * @brief Return a upper-case version of the string \b input . - * Numbers and special characters stay the same - * - * @param input The input string for which we want to ensure that all the characters are in upper case. - * @return std::string A upper-case version of the string \b input, where - * numbers and special characters stay the same - */ + /** + * @brief Return a upper-case version of the string \b input . + * Numbers and special characters stay the same + * + * @param input The input string for which we want to ensure that all the characters are in upper case. + * @return std::string A upper-case version of the string \b input, where + * numbers and special characters stay the same + */ std::string vpIoTools::toUpperCase(const std::string &input) { std::string out; @@ -2155,16 +2172,16 @@ std::string vpIoTools::toUpperCase(const std::string &input) out += std::toupper(*it); } return out; -} + } -/*! - Returns the absolute path using realpath() on Unix systems or - GetFullPathName() on Windows systems. \return According to realpath() - manual, returns an absolute pathname that names the same file, whose - resolution does not involve '.', '..', or symbolic links for Unix systems. - According to GetFullPathName() documentation, retrieves the full path of the - specified file for Windows systems. - */ + /*! + Returns the absolute path using realpath() on Unix systems or + GetFullPathName() on Windows systems. \return According to realpath() + manual, returns an absolute pathname that names the same file, whose + resolution does not involve '.', '..', or symbolic links for Unix systems. + According to GetFullPathName() documentation, retrieves the full path of the + specified file for Windows systems. + */ std::string vpIoTools::getAbsolutePathname(const std::string &pathname) { diff --git a/modules/core/src/tools/geometry/vpPlane.cpp b/modules/core/src/tools/geometry/vpPlane.cpp index 6125e7bb1f..d2fdf7805e 100644 --- a/modules/core/src/tools/geometry/vpPlane.cpp +++ b/modules/core/src/tools/geometry/vpPlane.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -106,10 +105,13 @@ vpPlane::vpPlane(const vpPlane &P) : A(0), B(0), C(0), D(0) */ vpPlane::vpPlane(const vpPoint &P, const vpColVector &n, vpPlaneFrame frame) : A(0), B(0), C(0), D(0) { + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; // Equation of the plane is given by: - A = n[0]; - B = n[1]; - C = n[2]; + A = n[index_0]; + B = n[index_1]; + C = n[index_2]; if (frame == vpPlane::camera_frame) { D = -((A * P.get_X()) + (B * P.get_Y()) + (C * P.get_Z())); @@ -145,12 +147,15 @@ void vpPlane::init(const vpPlane &P) */ void vpPlane::init(const vpColVector &P, const vpColVector &n) { + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; // Equation of the plane is given by: - A = n[0]; - B = n[1]; - C = n[2]; + A = n[index_0]; + B = n[index_1]; + C = n[index_2]; - D = -((A * P[0]) + (B * P[1]) + (C * P[2])); + D = -((A * P[0]) + (B * P[1]) + (C * P[index_2])); } /*! @@ -169,35 +174,38 @@ void vpPlane::init(const vpPoint &P, const vpPoint &Q, const vpPoint &R, vpPlane vpColVector a(3); vpColVector b(3); vpColVector n(3); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; if (frame == vpPlane::camera_frame) { // Calculate vector corresponding to PQ - a[0] = P.get_X() - Q.get_X(); - a[1] = P.get_Y() - Q.get_Y(); - a[2] = P.get_Z() - Q.get_Z(); + a[index_0] = P.get_X() - Q.get_X(); + a[index_1] = P.get_Y() - Q.get_Y(); + a[index_2] = P.get_Z() - Q.get_Z(); // Calculate vector corresponding to PR - b[0] = P.get_X() - R.get_X(); - b[1] = P.get_Y() - R.get_Y(); - b[2] = P.get_Z() - R.get_Z(); + b[index_0] = P.get_X() - R.get_X(); + b[index_1] = P.get_Y() - R.get_Y(); + b[index_2] = P.get_Z() - R.get_Z(); } else { - // Calculate vector corresponding to PQ - a[0] = P.get_oX() - Q.get_oX(); - a[1] = P.get_oY() - Q.get_oY(); - a[2] = P.get_oZ() - Q.get_oZ(); + // Calculate vector corresponding to PQ + a[index_0] = P.get_oX() - Q.get_oX(); + a[index_1] = P.get_oY() - Q.get_oY(); + a[index_2] = P.get_oZ() - Q.get_oZ(); // Calculate vector corresponding to PR - b[0] = P.get_oX() - R.get_oX(); - b[1] = P.get_oY() - R.get_oY(); - b[2] = P.get_oZ() - R.get_oZ(); + b[index_0] = P.get_oX() - R.get_oX(); + b[index_1] = P.get_oY() - R.get_oY(); + b[index_2] = P.get_oZ() - R.get_oZ(); } // Calculate normal vector to plane PQ x PR n = vpColVector::cross(a, b); // Equation of the plane is given by: - A = n[0]; - B = n[1]; - C = n[2]; + A = n[index_0]; + B = n[index_1]; + C = n[index_2]; if (frame == vpPlane::camera_frame) { D = -((A * P.get_X()) + (B * P.get_Y()) + (C * P.get_Z())); } @@ -250,9 +258,12 @@ double vpPlane::computeZ(double x, double y) const vpColVector vpPlane::getNormal() const { vpColVector n(3); - n[0] = A; - n[1] = B; - n[2] = C; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + n[index_0] = A; + n[index_1] = B; + n[index_2] = C; return n; } @@ -270,9 +281,12 @@ vpColVector vpPlane::getNormal() const void vpPlane::getNormal(vpColVector &n) const { n.resize(3); - n[0] = A; - n[1] = B; - n[2] = C; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + n[index_0] = A; + n[index_1] = B; + n[index_2] = C; } /*! @@ -300,19 +314,21 @@ void vpPlane::projectionPointOnPlan(const vpPoint &P, vpPoint &Pproj) const double vpPlane::rayIntersection(const vpPoint &M0, const vpPoint &M1, vpColVector &H) const { - double k, scal; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; // --comment: if M0.get_X() diff 0 or M0.get_Y() diff 0 or M0.get_Z() diff 0 if ((std::fabs(M0.get_X()) > std::numeric_limits::epsilon()) || (std::fabs(M0.get_Y()) > std::numeric_limits::epsilon()) || (std::fabs(M0.get_Z()) > std::numeric_limits::epsilon())) { double R[3]; - R[0] = M1.get_X() - M0.get_X(); - R[1] = M1.get_Y() - M0.get_Y(); - R[2] = M1.get_Z() - M0.get_Z(); + R[index_0] = M1.get_X() - M0.get_X(); + R[index_1] = M1.get_Y() - M0.get_Y(); + R[index_2] = M1.get_Z() - M0.get_Z(); - scal = (getA() * R[0]) + (getB() * R[1]) + (getC() * R[2]); + scal = (getA() * R[index_0]) + (getB() * R[index_1]) + (getC() * R[index_2]); // --comment: if scal != 0 if (std::fabs(scal) > std::numeric_limits::epsilon()) { k = -((getA() * M0.get_X()) + (getB() * M0.get_Y()) + (getC() * M0.get_Z()) + getD()) / scal; @@ -321,9 +337,9 @@ double vpPlane::rayIntersection(const vpPoint &M0, const vpPoint &M1, vpColVecto k = 0; } - H[0] = M0.get_X() + (k * R[0]); - H[1] = M0.get_Y() + (k * R[1]); - H[2] = M0.get_Z() + (k * R[2]); + H[index_0] = M0.get_X() + (k * R[index_0]); + H[index_1] = M0.get_Y() + (k * R[index_1]); + H[index_2] = M0.get_Z() + (k * R[index_2]); } else { scal = (getA() * M1.get_X()) + (getB() * M1.get_Y()) + (getC() * M1.get_Z()); @@ -334,9 +350,9 @@ double vpPlane::rayIntersection(const vpPoint &M0, const vpPoint &M1, vpColVecto else { k = 0; } - H[0] = k * M1.get_X(); - H[1] = k * M1.get_Y(); - H[2] = k * M1.get_Z(); + H[index_0] = k * M1.get_X(); + H[index_1] = k * M1.get_Y(); + H[index_2] = k * M1.get_Z(); } return k; @@ -344,10 +360,12 @@ double vpPlane::rayIntersection(const vpPoint &M0, const vpPoint &M1, vpColVecto double vpPlane::getIntersection(const vpColVector &M1, vpColVector &H) const { - double k, scal; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; - scal = (A * M1[0]) + (B * M1[1]) + (C * M1[2]); + scal = (A * M1[index_0]) + (B * M1[index_1]) + (C * M1[index_2]); // --comment: if scal != 0 if (std::fabs(scal) > std::numeric_limits::epsilon()) { k = -getD() / scal; @@ -355,9 +373,9 @@ double vpPlane::getIntersection(const vpColVector &M1, vpColVector &H) const else { k = 0; } - H[0] = k * M1[0]; - H[1] = k * M1[1]; - H[2] = k * M1[2]; + H[index_0] = k * M1[index_0]; + H[index_1] = k * M1[index_1]; + H[index_2] = k * M1[index_2]; return k; } @@ -373,11 +391,18 @@ double vpPlane::getIntersection(const vpColVector &M1, vpColVector &H) const void vpPlane::changeFrame(const vpHomogeneousMatrix &cMo) { // Save current plane parameters - double Ao = A, Bo = B, Co = C, Do = D; - A = (cMo[0][0] * Ao) + (cMo[0][1] * Bo) + (cMo[0][2] * Co); - B = (cMo[1][0] * Ao) + (cMo[1][1] * Bo) + (cMo[1][2] * Co); - C = (cMo[2][0] * Ao) + (cMo[2][1] * Bo) + (cMo[2][2] * Co); - D = Do - ((cMo[0][3] * A) + (cMo[1][3] * B) + (cMo[2][3] * C)); + double Ao = A; + double Bo = B; + double Co = C; + double Do = D; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + A = (cMo[index_0][0] * Ao) + (cMo[index_0][1] * Bo) + (cMo[index_0][index_2] * Co); + B = (cMo[index_1][0] * Ao) + (cMo[index_1][1] * Bo) + (cMo[index_1][index_2] * Co); + C = (cMo[index_2][0] * Ao) + (cMo[index_2][1] * Bo) + (cMo[index_2][index_2] * Co); + D = Do - ((cMo[index_0][index_3] * A) + (cMo[index_1][index_3] * B) + (cMo[index_2][index_3] * C)); } /*! diff --git a/modules/core/src/tools/geometry/vpPolygon.cpp b/modules/core/src/tools/geometry/vpPolygon.cpp index a7df252f0d..c5fa5946b0 100644 --- a/modules/core/src/tools/geometry/vpPolygon.cpp +++ b/modules/core/src/tools/geometry/vpPolygon.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/core/src/tools/geometry/vpRect.cpp b/modules/core/src/tools/geometry/vpRect.cpp index 2bcde5ceb6..7b958ac7c0 100644 --- a/modules/core/src/tools/geometry/vpRect.cpp +++ b/modules/core/src/tools/geometry/vpRect.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -39,7 +38,6 @@ \brief Defines a rectangle in the plane. */ -#include #include BEGIN_VISP_NAMESPACE diff --git a/modules/core/src/tools/geometry/vpRectOriented.cpp b/modules/core/src/tools/geometry/vpRectOriented.cpp index 58139e6a83..f2b482f190 100644 --- a/modules/core/src/tools/geometry/vpRectOriented.cpp +++ b/modules/core/src/tools/geometry/vpRectOriented.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/core/src/tools/geometry/vpXmlParserRectOriented.cpp b/modules/core/src/tools/geometry/vpXmlParserRectOriented.cpp index 386f83c8b5..93d9d51d04 100644 --- a/modules/core/src/tools/geometry/vpXmlParserRectOriented.cpp +++ b/modules/core/src/tools/geometry/vpXmlParserRectOriented.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,7 @@ * * Description: * XML parser to load and save oriented rectangle in a XML file - * - * Authors: - * Marc Pouliquen - * -*****************************************************************************/ + */ /*! \file vpXmlParserRectOriented.cpp diff --git a/modules/core/src/tools/histogram/vpHistogram.cpp b/modules/core/src/tools/histogram/vpHistogram.cpp index 415c49653b..e36b39fe7f 100644 --- a/modules/core/src/tools/histogram/vpHistogram.cpp +++ b/modules/core/src/tools/histogram/vpHistogram.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -528,7 +527,6 @@ void vpHistogram::display(const vpImage &I, const vpColor &color, void vpHistogram::smooth(unsigned int fsize) { if (m_histogram == nullptr) { - vpERROR_TRACE("Histogram array not initialised\n"); throw(vpImageException(vpImageException::notInitializedError, "Histogram array not initialised")); } @@ -568,7 +566,6 @@ void vpHistogram::smooth(unsigned int fsize) unsigned vpHistogram::getPeaks(std::list &peaks) { if (m_histogram == nullptr) { - vpERROR_TRACE("Histogram array not initialised\n"); throw(vpImageException(vpImageException::notInitializedError, "Histogram array not initialised")); } @@ -590,7 +587,6 @@ unsigned vpHistogram::getPeaks(std::list &peaks) if ((prev_slope > 0) && (next_slope == 0)) { sum_level += i + 1; ++cpt; - // continue; } else { // Peak detection @@ -844,7 +840,6 @@ bool vpHistogram::getPeaks(unsigned char dist, vpHistogramPeak &peakl, vpHistogr unsigned vpHistogram::getValey(std::list &valey) { if (m_histogram == nullptr) { - vpERROR_TRACE("Histogram array not initialised\n"); throw(vpImageException(vpImageException::notInitializedError, "Histogram array not initialised")); } diff --git a/modules/core/src/tools/histogram/vpHistogramPeak.cpp b/modules/core/src/tools/histogram/vpHistogramPeak.cpp index c738a4e7f4..5528f3fb97 100644 --- a/modules/core/src/tools/histogram/vpHistogramPeak.cpp +++ b/modules/core/src/tools/histogram/vpHistogramPeak.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/core/src/tools/histogram/vpHistogramValey.cpp b/modules/core/src/tools/histogram/vpHistogramValey.cpp index 6c61700c84..a4bedb3a46 100644 --- a/modules/core/src/tools/histogram/vpHistogramValey.cpp +++ b/modules/core/src/tools/histogram/vpHistogramValey.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/core/src/tools/time/vpTime.cpp b/modules/core/src/tools/time/vpTime.cpp index b735961e7e..beaa304f7f 100644 --- a/modules/core/src/tools/time/vpTime.cpp +++ b/modules/core/src/tools/time/vpTime.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -40,7 +39,6 @@ #include -#include #include // https://devblogs.microsoft.com/cppblog/c14-stl-features-fixes-and-breaking-changes-in-visual-studio-14-ctp1/ @@ -211,17 +209,17 @@ int wait(double t0, double t) "vpTime::wait() is not implemented on Windows Phone 8.0")); #endif #endif - } } +} - /*! - Wait t miliseconds from now. +/*! + Wait t miliseconds from now. - The waiting is done by a call to usleep() if the time to wait is greater - than vpTime::minTimeForUsleepCall. + The waiting is done by a call to usleep() if the time to wait is greater + than vpTime::minTimeForUsleepCall. - \param t : Time to wait in ms. - */ + \param t : Time to wait in ms. +*/ void wait(double t) { double timeToWait = t; @@ -262,14 +260,14 @@ void wait(double t) "vpTime::wait() is not implemented on Windows Phone 8.0")); #endif #endif - } } +} - /*! - Sleep t miliseconds from now. +/*! + Sleep t miliseconds from now. - \param t : Time to sleep in ms. - */ + \param t : Time to sleep in ms. +*/ void sleepMs(double t) { #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX diff --git a/modules/core/src/tracking/forward-projection/vpCircle.cpp b/modules/core/src/tracking/forward-projection/vpCircle.cpp index 024c400e6d..f6d47b8253 100644 --- a/modules/core/src/tracking/forward-projection/vpCircle.cpp +++ b/modules/core/src/tracking/forward-projection/vpCircle.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -69,13 +68,20 @@ void vpCircle::setWorldCoordinates(const vpColVector &oP_) { this->oP = oP_; } */ void vpCircle::setWorldCoordinates(double oA, double oB, double oC, double oX, double oY, double oZ, double R) { - oP[0] = oA; - oP[1] = oB; - oP[2] = oC; - oP[3] = oX; - oP[4] = oY; - oP[5] = oZ; - oP[6] = R; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + const unsigned int index_5 = 5; + const unsigned int index_6 = 6; + oP[index_0] = oA; + oP[index_1] = oB; + oP[index_2] = oC; + oP[index_3] = oX; + oP[index_4] = oY; + oP[index_5] = oZ; + oP[index_6] = R; } /*! @@ -159,16 +165,22 @@ void vpCircle::projection(const vpColVector &cP_, vpColVector &p_) const { double det_threshold = 1e-10; p_.resize(5, false); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + const unsigned int index_5 = 5; vpColVector K(6); { - double A = cP_[0]; - double B = cP_[1]; - double C = cP_[2]; + double A = cP_[index_0]; + double B = cP_[index_1]; + double C = cP_[index_2]; - double X0 = cP_[3]; - double Y0 = cP_[4]; - double Z0 = cP_[5]; + double X0 = cP_[index_3]; + double Y0 = cP_[index_4]; + double Z0 = cP_[index_5]; double r = cP_[6]; @@ -179,28 +191,28 @@ void vpCircle::projection(const vpColVector &cP_, vpColVector &p_) const B = B / det; C = C / det; - K[0] = (1 - (2 * A * X0)) + (A * A * s); - K[1] = (1 - (2 * B * Y0)) + (B * B * s); - K[2] = ((-A * Y0) - (B * X0)) + (A * B * s); - K[3] = ((-C * X0) - (A * Z0)) + (A * C * s); - K[4] = ((-C * Y0) - (B * Z0)) + (B * C * s); - K[5] = (1 - (2 * C * Z0)) + (C * C * s); + K[index_0] = (1 - (2 * A * X0)) + (A * A * s); + K[index_1] = (1 - (2 * B * Y0)) + (B * B * s); + K[index_2] = ((-A * Y0) - (B * X0)) + (A * B * s); + K[index_3] = ((-C * X0) - (A * Z0)) + (A * C * s); + K[index_4] = ((-C * Y0) - (B * Z0)) + (B * C * s); + K[index_5] = (1 - (2 * C * Z0)) + (C * C * s); } - double det = (K[2] * K[2]) - (K[0] * K[1]); + double det = (K[index_2] * K[index_2]) - (K[index_0] * K[index_1]); if (fabs(det) < det_threshold) { throw(vpException(vpException::divideByZeroError, "Division by 0 in vpCircle::projection.")); } - double xc = ((K[1] * K[3]) - (K[2] * K[4])) / det; - double yc = ((K[0] * K[4]) - (K[2] * K[3])) / det; + double xc = ((K[index_1] * K[index_3]) - (K[index_2] * K[index_4])) / det; + double yc = ((K[index_0] * K[index_4]) - (K[index_2] * K[index_3])) / det; - double c = sqrt(((K[0] - K[1]) * (K[0] - K[1])) + (4 * K[2] * K[2])); - double s = 2 * (((K[0] * xc * xc) + (2 * K[2] * xc * yc) + (K[1] * yc * yc)) - K[5]); + double c = sqrt(((K[index_0] - K[index_1]) * (K[index_0] - K[index_1])) + (4 * K[index_2] * K[index_2])); + double s = 2 * (((K[index_0] * xc * xc) + (2 * K[index_2] * xc * yc) + (K[1] * yc * yc)) - K[index_5]); double A, B, E; - if (fabs(K[2]) < std::numeric_limits::epsilon()) { + if (fabs(K[index_2]) < std::numeric_limits::epsilon()) { E = 0.0; if (K[0] > K[1]) { A = sqrt(s / ((K[0] + K[1]) + c)); @@ -212,7 +224,7 @@ void vpCircle::projection(const vpColVector &cP_, vpColVector &p_) const } } else { - E = ((K[1] - K[0]) + c) / (2 * K[2]); + E = ((K[1] - K[0]) + c) / (2 * K[index_2]); if (fabs(E) > 1.0) { A = sqrt(s / ((K[0] + K[1]) + c)); B = sqrt(s / ((K[0] + K[1]) - c)); @@ -230,11 +242,11 @@ void vpCircle::projection(const vpColVector &cP_, vpColVector &p_) const double n11 = ((vpMath::sqr(A) - vpMath::sqr(B)) * E) / det; double n02 = (vpMath::sqr(B) + vpMath::sqr(A * E)) / det; - p_[0] = xc; - p_[1] = yc; - p_[2] = n20; - p_[3] = n11; - p_[4] = n02; + p_[index_0] = xc; + p_[index_1] = yc; + p_[index_2] = n20; + p_[index_3] = n11; + p_[index_4] = n02; } /*! @@ -252,25 +264,32 @@ void vpCircle::changeFrame(const vpHomogeneousMatrix &noMo, vpColVector &noP) co noP.resize(7, false); double A, B, C; - A = (noMo[0][0] * oP[0]) + (noMo[0][1] * oP[1]) + (noMo[0][2] * oP[2]); - B = (noMo[1][0] * oP[0]) + (noMo[1][1] * oP[1]) + (noMo[1][2] * oP[2]); - C = (noMo[2][0] * oP[0]) + (noMo[2][1] * oP[1]) + (noMo[2][2] * oP[2]); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + const unsigned int index_5 = 5; + const unsigned int index_6 = 6; + A = (noMo[index_0][0] * oP[0]) + (noMo[index_0][1] * oP[1]) + (noMo[index_0][index_2] * oP[index_2]); + B = (noMo[index_1][0] * oP[0]) + (noMo[index_1][1] * oP[1]) + (noMo[index_1][index_2] * oP[index_2]); + C = (noMo[index_2][0] * oP[0]) + (noMo[index_2][1] * oP[1]) + (noMo[index_2][index_2] * oP[index_2]); double X0, Y0, Z0; - X0 = noMo[0][3] + (noMo[0][0] * oP[3]) + (noMo[0][1] * oP[4]) + (noMo[0][2] * oP[5]); - Y0 = noMo[1][3] + (noMo[1][0] * oP[3]) + (noMo[1][1] * oP[4]) + (noMo[1][2] * oP[5]); - Z0 = noMo[2][3] + (noMo[2][0] * oP[3]) + (noMo[2][1] * oP[4]) + (noMo[2][2] * oP[5]); + X0 = noMo[index_0][index_3] + (noMo[index_0][0] * oP[3]) + (noMo[index_0][1] * oP[index_4]) + (noMo[index_0][index_2] * oP[index_5]); + Y0 = noMo[index_1][index_3] + (noMo[index_1][0] * oP[3]) + (noMo[index_1][1] * oP[index_4]) + (noMo[index_1][index_2] * oP[index_5]); + Z0 = noMo[index_2][index_3] + (noMo[index_2][0] * oP[3]) + (noMo[index_2][1] * oP[index_4]) + (noMo[index_2][index_2] * oP[index_5]); double R = oP[6]; - noP[0] = A; - noP[1] = B; - noP[2] = C; + noP[index_0] = A; + noP[index_1] = B; + noP[index_2] = C; - noP[3] = X0; - noP[4] = Y0; - noP[5] = Z0; + noP[index_3] = X0; + noP[index_4] = Y0; + noP[index_5] = Z0; - noP[6] = R; + noP[index_6] = R; } /*! @@ -282,25 +301,32 @@ void vpCircle::changeFrame(const vpHomogeneousMatrix &noMo, vpColVector &noP) co void vpCircle::changeFrame(const vpHomogeneousMatrix &cMo) { double A, B, C; - A = (cMo[0][0] * oP[0]) + (cMo[0][1] * oP[1]) + (cMo[0][2] * oP[2]); - B = (cMo[1][0] * oP[0]) + (cMo[1][1] * oP[1]) + (cMo[1][2] * oP[2]); - C = (cMo[2][0] * oP[0]) + (cMo[2][1] * oP[1]) + (cMo[2][2] * oP[2]); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + const unsigned int index_5 = 5; + const unsigned int index_6 = 6; + A = (cMo[index_0][0] * oP[0]) + (cMo[index_0][1] * oP[1]) + (cMo[index_0][index_2] * oP[index_2]); + B = (cMo[index_1][0] * oP[0]) + (cMo[index_1][1] * oP[1]) + (cMo[index_1][index_2] * oP[index_2]); + C = (cMo[index_2][0] * oP[0]) + (cMo[index_2][1] * oP[1]) + (cMo[index_2][index_2] * oP[index_2]); double X0, Y0, Z0; - X0 = cMo[0][3] + (cMo[0][0] * oP[3]) + (cMo[0][1] * oP[4]) + (cMo[0][2] * oP[5]); - Y0 = cMo[1][3] + (cMo[1][0] * oP[3]) + (cMo[1][1] * oP[4]) + (cMo[1][2] * oP[5]); - Z0 = cMo[2][3] + (cMo[2][0] * oP[3]) + (cMo[2][1] * oP[4]) + (cMo[2][2] * oP[5]); + X0 = cMo[index_0][index_3] + (cMo[index_0][0] * oP[index_3]) + (cMo[index_0][1] * oP[index_4]) + (cMo[index_0][index_2] * oP[index_5]); + Y0 = cMo[index_1][index_3] + (cMo[index_1][0] * oP[index_3]) + (cMo[index_1][1] * oP[index_4]) + (cMo[index_1][index_2] * oP[index_5]); + Z0 = cMo[index_2][index_3] + (cMo[index_2][0] * oP[index_3]) + (cMo[index_2][1] * oP[index_4]) + (cMo[index_2][index_2] * oP[index_5]); double R = oP[6]; - cP[0] = A; - cP[1] = B; - cP[2] = C; + cP[index_0] = A; + cP[index_1] = B; + cP[index_2] = C; - cP[3] = X0; - cP[4] = Y0; - cP[5] = Z0; + cP[index_3] = X0; + cP[index_4] = Y0; + cP[index_5] = Z0; - cP[6] = R; + cP[index_6] = R; } /*! @@ -315,7 +341,12 @@ void vpCircle::changeFrame(const vpHomogeneousMatrix &cMo) void vpCircle::display(const vpImage &I, const vpCameraParameters &cam, const vpColor &color, unsigned int thickness) { - vpFeatureDisplay::displayEllipse(p[0], p[1], p[2], p[3], p[4], cam, I, color, thickness); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + vpFeatureDisplay::displayEllipse(p[index_0], p[index_1], p[index_2], p[index_3], p[index_4], cam, I, color, thickness); } /*! @@ -330,7 +361,12 @@ void vpCircle::display(const vpImage &I, const vpCameraParameters void vpCircle::display(const vpImage &I, const vpCameraParameters &cam, const vpColor &color, unsigned int thickness) { - vpFeatureDisplay::displayEllipse(p[0], p[1], p[2], p[3], p[4], cam, I, color, thickness); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + vpFeatureDisplay::displayEllipse(p[index_0], p[index_1], p[index_2], p[index_3], p[index_4], cam, I, color, thickness); } /*! @@ -350,7 +386,12 @@ void vpCircle::display(const vpImage &I, const vpHomogeneousMatri vpColVector v_cP, v_p; changeFrame(cMo, v_cP); projection(v_cP, v_p); - vpFeatureDisplay::displayEllipse(v_p[0], v_p[1], v_p[2], v_p[3], v_p[4], cam, I, color, thickness); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + vpFeatureDisplay::displayEllipse(v_p[index_0], v_p[index_1], v_p[index_2], v_p[index_3], v_p[index_4], cam, I, color, thickness); } /*! @@ -370,7 +411,12 @@ void vpCircle::display(const vpImage &I, const vpHomogeneousMatrix &cMo, vpColVector v_cP, v_p; changeFrame(cMo, v_cP); projection(v_cP, v_p); - vpFeatureDisplay::displayEllipse(v_p[0], v_p[1], v_p[2], v_p[3], v_p[4], cam, I, color, thickness); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + vpFeatureDisplay::displayEllipse(v_p[index_0], v_p[index_1], v_p[index_2], v_p[index_3], v_p[index_4], cam, I, color, thickness); } //! For memory issue (used by the vpServo class only) @@ -406,11 +452,17 @@ void vpCircle::computeIntersectionPoint(const vpCircle &circle, const vpCameraPa double u0 = cam.get_u0(); double v0 = cam.get_v0(); - double n11 = circle.p[3]; - double n02 = circle.p[4]; - double n20 = circle.p[2]; - double Xg = u0 + (circle.p[0] * px); - double Yg = v0 + (circle.p[1] * py); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + + double n11 = circle.p[index_3]; + double n02 = circle.p[index_4]; + double n20 = circle.p[index_2]; + double Xg = u0 + (circle.p[index_0] * px); + double Yg = v0 + (circle.p[index_1] * py); // Find Intersection between line and ellipse in the image. diff --git a/modules/core/src/tracking/forward-projection/vpForwardProjection.cpp b/modules/core/src/tracking/forward-projection/vpForwardProjection.cpp index 247d985251..a5c26d8019 100644 --- a/modules/core/src/tracking/forward-projection/vpForwardProjection.cpp +++ b/modules/core/src/tracking/forward-projection/vpForwardProjection.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -38,7 +37,6 @@ \brief class that defines what is a point */ -#include #include BEGIN_VISP_NAMESPACE diff --git a/modules/core/src/tracking/forward-projection/vpLine.cpp b/modules/core/src/tracking/forward-projection/vpLine.cpp index 168461f95f..da7b23826d 100644 --- a/modules/core/src/tracking/forward-projection/vpLine.cpp +++ b/modules/core/src/tracking/forward-projection/vpLine.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -40,7 +39,6 @@ #include -#include #include #include @@ -54,9 +52,11 @@ BEGIN_VISP_NAMESPACE */ void vpLine::init() { - oP.resize(8); - cP.resize(8); - p.resize(2); + const unsigned int val_2 = 2; + const unsigned int val_8 = 8; + oP.resize(val_8); + cP.resize(val_8); + p.resize(val_2); } /*! @@ -83,15 +83,23 @@ vpLine::vpLine() { init(); } void vpLine::setWorldCoordinates(const double &oA1, const double &oB1, const double &oC1, const double &oD1, const double &oA2, const double &oB2, const double &oC2, const double &oD2) { - oP[0] = oA1; - oP[1] = oB1; - oP[2] = oC1; - oP[3] = oD1; - - oP[4] = oA2; - oP[5] = oB2; - oP[6] = oC2; - oP[7] = oD2; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + const unsigned int index_5 = 5; + const unsigned int index_6 = 6; + const unsigned int index_7 = 7; + oP[index_0] = oA1; + oP[index_1] = oB1; + oP[index_2] = oC1; + oP[index_3] = oD1; + + oP[index_4] = oA2; + oP[index_5] = oB2; + oP[index_6] = oC2; + oP[index_7] = oD2; } /*! @@ -149,7 +157,8 @@ void vpLine::setWorldCoordinates(const vpColVector &oP1, const vpColVector &oP2) if (oP2.getRows() != 4) { throw vpException(vpException::dimensionError, "Size of oP2 is not equal to 4 as it should be"); } - for (unsigned int i = 0; i < 4; ++i) { + const unsigned int val_4 = 4; + for (unsigned int i = 0; i < val_4; ++i) { oP[i] = oP1[i]; oP[i + 4] = oP2[i]; } @@ -217,16 +226,24 @@ void vpLine::projection(const vpColVector &cP_, vpColVector &p_) const throw vpException(vpException::dimensionError, "Size of cP is not equal to 8 as it should be"); } double A1, A2, B1, B2, C1, C2, D1, D2; - - A1 = cP_[0]; - B1 = cP_[1]; - C1 = cP_[2]; - D1 = cP_[3]; - - A2 = cP_[4]; - B2 = cP_[5]; - C2 = cP_[6]; - D2 = cP_[7]; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + const unsigned int index_5 = 5; + const unsigned int index_6 = 6; + const unsigned int index_7 = 7; + + A1 = cP_[index_0]; + B1 = cP_[index_1]; + C1 = cP_[index_2]; + D1 = cP_[index_3]; + + A2 = cP_[index_4]; + B2 = cP_[index_5]; + C2 = cP_[index_6]; + D2 = cP_[index_7]; double a, b, c, s; a = (A2 * D1) - (A1 * D2); @@ -331,29 +348,37 @@ void vpLine::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_) const double a1, a2, b1, b2, c1, c2, d1, d2; double A1, A2, B1, B2, C1, C2, D1, D2; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + const unsigned int index_5 = 5; + const unsigned int index_6 = 6; + const unsigned int index_7 = 7; // in case of verification // double x,y,z,ap1,ap2,bp1,bp2,cp1,cp2,dp1,dp2; - a1 = oP[0]; - b1 = oP[1]; - c1 = oP[2]; - d1 = oP[3]; + a1 = oP[index_0]; + b1 = oP[index_1]; + c1 = oP[index_2]; + d1 = oP[index_3]; - a2 = oP[4]; - b2 = oP[5]; - c2 = oP[6]; - d2 = oP[7]; + a2 = oP[index_4]; + b2 = oP[index_5]; + c2 = oP[index_6]; + d2 = oP[index_7]; - A1 = (cMo[0][0] * a1) + (cMo[0][1] * b1) + (cMo[0][2] * c1); - B1 = (cMo[1][0] * a1) + (cMo[1][1] * b1) + (cMo[1][2] * c1); - C1 = (cMo[2][0] * a1) + (cMo[2][1] * b1) + (cMo[2][2] * c1); - D1 = d1 - ((cMo[0][3] * A1) + (cMo[1][3] * B1) + (cMo[2][3] * C1)); + A1 = (cMo[index_0][0] * a1) + (cMo[index_0][1] * b1) + (cMo[index_0][index_2] * c1); + B1 = (cMo[index_1][0] * a1) + (cMo[index_1][1] * b1) + (cMo[index_1][index_2] * c1); + C1 = (cMo[index_2][0] * a1) + (cMo[index_2][1] * b1) + (cMo[index_2][index_2] * c1); + D1 = d1 - ((cMo[index_0][index_3] * A1) + (cMo[index_1][index_3] * B1) + (cMo[index_2][index_3] * C1)); - A2 = (cMo[0][0] * a2) + (cMo[0][1] * b2) + (cMo[0][2] * c2); - B2 = (cMo[1][0] * a2) + (cMo[1][1] * b2) + (cMo[1][2] * c2); - C2 = (cMo[2][0] * a2) + (cMo[2][1] * b2) + (cMo[2][2] * c2); - D2 = d2 - ((cMo[0][3] * A2) + (cMo[1][3] * B2) + (cMo[2][3] * C2)); + A2 = (cMo[index_0][0] * a2) + (cMo[index_0][1] * b2) + (cMo[index_0][index_2] * c2); + B2 = (cMo[index_1][0] * a2) + (cMo[index_1][1] * b2) + (cMo[index_1][index_2] * c2); + C2 = (cMo[index_2][0] * a2) + (cMo[index_2][1] * b2) + (cMo[index_2][index_2] * c2); + D2 = d2 - ((cMo[index_0][index_3] * A2) + (cMo[index_1][index_3] * B2) + (cMo[index_2][index_3] * C2)); // Adding constraints on the straight line to have a unique representation @@ -380,11 +405,11 @@ void vpLine::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_) const A1 = a1 * d1; B1 = b1 * d1; C1 = c1 * d1; - cP_[0] = A1; - cP_[1] = B1; - cP_[2] = C1; + cP_[index_0] = A1; + cP_[index_1] = B1; + cP_[index_2] = C1; - cP_[3] = 0; + cP_[index_3] = 0; // Constraint A1 A2 + B1 B2 + C1 C2 = 0 (P2 orthogonal to P1) // N2_new = (N1 x N2) x N1_new @@ -411,8 +436,6 @@ void vpLine::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_) const C2 = -C2; D2 = -D2; } - // vpERROR_TRACE("A1 B1 C1 D1 %f %f %f %f ", A1, B1, C1, D1) ; - // vpERROR_TRACE("A2 B2 C2 D2 %f %f %f %f ", A2, B2, C2, D2) ; cP_[4] = A2; cP_[5] = B2; diff --git a/modules/core/src/tracking/forward-projection/vpPoint.cpp b/modules/core/src/tracking/forward-projection/vpPoint.cpp index a684f74d7c..19d62eee9b 100644 --- a/modules/core/src/tracking/forward-projection/vpPoint.cpp +++ b/modules/core/src/tracking/forward-projection/vpPoint.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -38,22 +37,23 @@ \brief Class that defines what is a 3D point. */ -#include #include #include BEGIN_VISP_NAMESPACE void vpPoint::init() { + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; p.resize(3); p = 0; - p[2] = 1; + p[index_2] = 1.; oP.resize(4); - oP = 0; - oP[3] = 1; + oP = 0.; + oP[index_3] = 1.; cP.resize(4); - cP = 0; - cP[3] = 1; + cP = 0.; + cP[index_3] = 1.; // default value Z (1 meters) set_Z(1); @@ -110,10 +110,14 @@ vpPoint::vpPoint(const std::vector &oP_) */ void vpPoint::setWorldCoordinates(double oX, double oY, double oZ) { - oP[0] = oX; - oP[1] = oY; - oP[2] = oZ; - oP[3] = 1; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + oP[index_0] = oX; + oP[index_1] = oY; + oP[index_2] = oZ; + oP[index_3] = 1; } /*! @@ -128,18 +132,25 @@ void vpPoint::setWorldCoordinates(double oX, double oY, double oZ) */ void vpPoint::setWorldCoordinates(const vpColVector &oP_) { - if (oP_.size() == 3) { - oP[0] = oP_[0]; - oP[1] = oP_[1]; - oP[2] = oP_[2]; - oP[3] = 1.; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int val_3 = 3; + const unsigned int val_4 = 4; + + if (oP_.size() == val_3) { + oP[index_0] = oP_[index_0]; + oP[index_1] = oP_[index_1]; + oP[index_2] = oP_[index_2]; + oP[index_3] = 1.; } - else if (oP_.size() == 4) { - oP[0] = oP_[0]; - oP[1] = oP_[1]; - oP[2] = oP_[2]; - oP[3] = oP_[3]; - oP /= oP[3]; + else if (oP_.size() == val_4) { + oP[index_0] = oP_[index_0]; + oP[index_1] = oP_[index_1]; + oP[index_2] = oP_[index_2]; + oP[index_3] = oP_[index_3]; + oP /= oP[index_3]; } else { throw(vpException(vpException::dimensionError, "Cannot initialize vpPoint from vector with size %d", oP_.size())); @@ -158,18 +169,25 @@ void vpPoint::setWorldCoordinates(const vpColVector &oP_) */ void vpPoint::setWorldCoordinates(const std::vector &oP_) { - if (oP_.size() == 3) { - oP[0] = oP_[0]; - oP[1] = oP_[1]; - oP[2] = oP_[2]; - oP[3] = 1.; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int val_3 = 3; + const unsigned int val_4 = 4; + + if (oP_.size() == val_3) { + oP[index_0] = oP_[index_0]; + oP[index_1] = oP_[index_1]; + oP[index_2] = oP_[index_2]; + oP[index_3] = 1.; } - else if (oP_.size() == 4) { - oP[0] = oP_[0]; - oP[1] = oP_[1]; - oP[2] = oP_[2]; - oP[3] = oP_[3]; - oP /= oP[3]; + else if (oP_.size() == val_4) { + oP[index_0] = oP_[index_0]; + oP[index_1] = oP_[index_1]; + oP[index_2] = oP_[index_2]; + oP[index_3] = oP_[index_3]; + oP /= oP[index_3]; } else { throw(vpException(vpException::dimensionError, "Cannot initialize vpPoint from vector with size %d", oP_.size())); @@ -179,9 +197,12 @@ void vpPoint::setWorldCoordinates(const std::vector &oP_) //! Get the point object frame coordinates. void vpPoint::getWorldCoordinates(double &oX, double &oY, double &oZ) { - oX = oP[0]; - oY = oP[1]; - oZ = oP[2]; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + oX = oP[index_0]; + oY = oP[index_1]; + oZ = oP[index_2]; } /*! @@ -225,11 +246,14 @@ vpColVector vpPoint::getWorldCoordinates(void) { return this->oP; } */ void vpPoint::projection(const vpColVector &v_cP, vpColVector &v_p) const { + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; v_p.resize(3, false); - v_p[0] = v_cP[0] / v_cP[2]; - v_p[1] = v_cP[1] / v_cP[2]; - v_p[2] = 1; + v_p[index_0] = v_cP[index_0] / v_cP[index_2]; + v_p[index_1] = v_cP[index_1] / v_cP[index_2]; + v_p[index_2] = 1; } /*! @@ -242,18 +266,22 @@ void vpPoint::projection(const vpColVector &v_cP, vpColVector &v_p) const */ void vpPoint::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &v_cP) const { + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; v_cP.resize(4, false); - v_cP[0] = (cMo[0][0] * oP[0]) + (cMo[0][1] * oP[1]) + (cMo[0][2] * oP[2]) + (cMo[0][3] * oP[3]); - v_cP[1] = (cMo[1][0] * oP[0]) + (cMo[1][1] * oP[1]) + (cMo[1][2] * oP[2]) + (cMo[1][3] * oP[3]); - v_cP[2] = (cMo[2][0] * oP[0]) + (cMo[2][1] * oP[1]) + (cMo[2][2] * oP[2]) + (cMo[2][3] * oP[3]); - v_cP[3] = (cMo[3][0] * oP[0]) + (cMo[3][1] * oP[1]) + (cMo[3][2] * oP[2]) + (cMo[3][3] * oP[3]); + v_cP[index_0] = (cMo[index_0][index_0] * oP[index_0]) + (cMo[index_0][index_1] * oP[index_1]) + (cMo[index_0][index_2] * oP[index_2]) + (cMo[index_0][index_3] * oP[index_3]); + v_cP[index_1] = (cMo[index_1][index_0] * oP[index_0]) + (cMo[index_1][index_1] * oP[index_1]) + (cMo[index_1][index_2] * oP[index_2]) + (cMo[index_1][index_3] * oP[index_3]); + v_cP[index_2] = (cMo[index_2][index_0] * oP[index_0]) + (cMo[index_2][index_1] * oP[index_1]) + (cMo[index_2][index_2] * oP[index_2]) + (cMo[index_2][index_3] * oP[index_3]); + v_cP[index_3] = (cMo[index_3][index_0] * oP[index_0]) + (cMo[index_3][index_1] * oP[index_1]) + (cMo[index_3][index_2] * oP[index_2]) + (cMo[index_3][index_3] * oP[index_3]); - double d = 1 / v_cP[3]; - v_cP[0] *= d; - v_cP[1] *= d; - v_cP[2] *= d; - v_cP[3] *= d; + double d = 1 / v_cP[index_3]; + v_cP[index_0] *= d; + v_cP[index_1] *= d; + v_cP[index_2] *= d; + v_cP[index_3] *= d; } /*! @@ -266,93 +294,22 @@ void vpPoint::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &v_cP) con */ void vpPoint::changeFrame(const vpHomogeneousMatrix &cMo) { - double X = (cMo[0][0] * oP[0]) + (cMo[0][1] * oP[1]) + (cMo[0][2] * oP[2]) + (cMo[0][3] * oP[3]); - double Y = (cMo[1][0] * oP[0]) + (cMo[1][1] * oP[1]) + (cMo[1][2] * oP[2]) + (cMo[1][3] * oP[3]); - double Z = (cMo[2][0] * oP[0]) + (cMo[2][1] * oP[1]) + (cMo[2][2] * oP[2]) + (cMo[2][3] * oP[3]); - double W = (cMo[3][0] * oP[0]) + (cMo[3][1] * oP[1]) + (cMo[3][2] * oP[2]) + (cMo[3][3] * oP[3]); - - double d = 1 / W; - cP[0] = X * d; - cP[1] = Y * d; - cP[2] = Z * d; - cP[3] = 1; -} - -#if 0 -/*! - From the coordinates of the point in camera frame b and the transformation between - camera frame a and camera frame b computes the coordinates of the point in camera frame a. - - \param aMb : 3D transformation between camera frame a and b. - \param bP : 3D coordinates of the point in camera frame bP. - - \return A point with 3D coordinates in the camera frame a. The coordinates in the world or object - frame are set to the same coordinates than the one in the camera frame. -*/ -const vpPoint -operator*(const vpHomogeneousMatrix &aMb, const vpPoint &bP) -{ - vpPoint aP; - - vpColVector v(4), v1(4); - - v[0] = bP.get_X(); - v[1] = bP.get_Y(); - v[2] = bP.get_Z(); - v[3] = bP.get_W(); - - v1[0] = aMb[0][0]*v[0] + aMb[0][1]*v[1]+ aMb[0][2]*v[2]+ aMb[0][3]*v[3]; - v1[1] = aMb[1][0]*v[0] + aMb[1][1]*v[1]+ aMb[1][2]*v[2]+ aMb[1][3]*v[3]; - v1[2] = aMb[2][0]*v[0] + aMb[2][1]*v[1]+ aMb[2][2]*v[2]+ aMb[2][3]*v[3]; - v1[3] = aMb[3][0]*v[0] + aMb[3][1]*v[1]+ aMb[3][2]*v[2]+ aMb[3][3]*v[3]; - - v1 /= v1[3]; - - // v1 = M*v ; - aP.set_X(v1[0]); - aP.set_Y(v1[1]); - aP.set_Z(v1[2]); - aP.set_W(v1[3]); - - aP.set_oX(v1[0]); - aP.set_oY(v1[1]); - aP.set_oZ(v1[2]); - aP.set_oW(v1[3]); - - return aP; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + double X = (cMo[index_0][index_0] * oP[index_0]) + (cMo[index_0][index_1] * oP[index_1]) + (cMo[index_0][index_2] * oP[index_2]) + (cMo[index_0][index_3] * oP[index_3]); + double Y = (cMo[index_1][index_0] * oP[index_0]) + (cMo[index_1][index_1] * oP[index_1]) + (cMo[index_1][index_2] * oP[index_2]) + (cMo[index_1][index_3] * oP[index_3]); + double Z = (cMo[index_2][index_0] * oP[index_0]) + (cMo[index_2][index_1] * oP[index_1]) + (cMo[index_2][index_2] * oP[index_2]) + (cMo[index_2][index_3] * oP[index_3]); + double W = (cMo[index_3][index_0] * oP[index_0]) + (cMo[index_3][index_1] * oP[index_1]) + (cMo[index_3][index_2] * oP[index_2]) + (cMo[index_3][index_3] * oP[index_3]); + + double d = 1. / W; + cP[index_0] = X * d; + cP[index_1] = Y * d; + cP[index_2] = Z * d; + cP[index_3] = 1.; } -/*! - From the coordinates of the point in image plane b and the homography between image - a and b computes the coordinates of the point in image plane a. - - \param aHb : Homography between image a and b. - \param bP : 2D coordinates of the point in the image plane b. - - \return A point with 2D coordinates in the image plane a. -*/ -const vpPoint -operator*(const vpHomography &aHb, const vpPoint &bP) -{ - vpPoint aP; - vpColVector v(3), v1(3); - - v[0] = bP.get_x(); - v[1] = bP.get_y(); - v[2] = bP.get_w(); - - v1[0] = aHb[0][0]*v[0] + aHb[0][1]*v[1]+ aHb[0][2]*v[2]; - v1[1] = aHb[1][0]*v[0] + aHb[1][1]*v[1]+ aHb[1][2]*v[2]; - v1[2] = aHb[2][0]*v[0] + aHb[2][1]*v[1]+ aHb[2][2]*v[2]; - - // v1 = M*v ; - aP.set_x(v1[0]); - aP.set_y(v1[1]); - aP.set_w(v1[2]); - - return aP; -} -#endif //! For memory issue (used by the vpServo class only). vpPoint *vpPoint::duplicate() const { @@ -374,11 +331,11 @@ vpPoint *vpPoint::duplicate() const void vpPoint::display(const vpImage &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &color, unsigned int thickness) { - vpColVector v_cP, v_p; changeFrame(cMo, v_cP); + const unsigned int index_2 = 2; - if (v_cP[2] < 0) { // no display if point is behind the camera + if (v_cP[index_2] < 0) { // no display if point is behind the camera return; } @@ -402,8 +359,9 @@ void vpPoint::display(const vpImage &I, const vpHomogeneousMatrix &cMo, { vpColVector v_cP, v_p; changeFrame(cMo, v_cP); + const unsigned int index_2 = 2; - if (v_cP[2] < 0) { // no display if point is behind the camera + if (v_cP[index_2] < 0) { // no display if point is behind the camera return; } @@ -441,29 +399,29 @@ void vpPoint::display(const vpImage &I, const vpCameraParameters &cam, c // Get coordinates //! Get the point cX coordinate in the camera frame. -double vpPoint::get_X() const { return cP[0]; } +double vpPoint::get_X() const { const unsigned int index_0 = 0; return cP[index_0]; } //! Get the point cY coordinate in the camera frame. -double vpPoint::get_Y() const { return cP[1]; } +double vpPoint::get_Y() const { const unsigned int index_1 = 1; return cP[index_1]; } //! Get the point cZ coordinate in the camera frame. -double vpPoint::get_Z() const { return cP[2]; } +double vpPoint::get_Z() const { const unsigned int index_2 = 2; return cP[index_2]; } //! Get the point cW coordinate in the camera frame. -double vpPoint::get_W() const { return cP[3]; } +double vpPoint::get_W() const { const unsigned int index_3 = 3; return cP[index_3]; } //! Get the point oX coordinate in the object frame. -double vpPoint::get_oX() const { return oP[0]; } +double vpPoint::get_oX() const { const unsigned int index_0 = 0; return oP[index_0]; } //! Get the point oY coordinate in the object frame. -double vpPoint::get_oY() const { return oP[1]; } +double vpPoint::get_oY() const { const unsigned int index_1 = 1; return oP[index_1]; } //! Get the point oZ coordinate in the object frame. -double vpPoint::get_oZ() const { return oP[2]; } +double vpPoint::get_oZ() const { const unsigned int index_2 = 2; return oP[index_2]; } //! Get the point oW coordinate in the object frame. -double vpPoint::get_oW() const { return oP[3]; } +double vpPoint::get_oW() const { const unsigned int index_3 = 3; return oP[index_3]; } //! Get the point x coordinate in the image plane. -double vpPoint::get_x() const { return p[0]; } +double vpPoint::get_x() const { const unsigned int index_0 = 0; return p[index_0]; } //! Get the point y coordinate in the image plane. -double vpPoint::get_y() const { return p[1]; } +double vpPoint::get_y() const { const unsigned int index_1 = 1; return p[index_1]; } //! Get the point w coordinate in the image plane. -double vpPoint::get_w() const { return p[2]; } +double vpPoint::get_w() const { const unsigned int index_2 = 2; return p[index_2]; } /*! Perspective projection of the 3D point. @@ -475,36 +433,39 @@ double vpPoint::get_w() const { return p[2]; } */ void vpPoint::projection() { - double d = 1 / cP[2]; - p[0] = cP[0] * d; - p[1] = cP[1] * d; - p[2] = 1; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + double d = 1 / cP[index_2]; + p[index_0] = cP[index_0] * d; + p[index_1] = cP[index_1] * d; + p[index_2] = 1; } //! Set the point cX coordinate in the camera frame. -void vpPoint::set_X(double cX) { cP[0] = cX; } +void vpPoint::set_X(double cX) { const unsigned int index_0 = 0; cP[index_0] = cX; } //! Set the point cY coordinate in the camera frame. -void vpPoint::set_Y(double cY) { cP[1] = cY; } +void vpPoint::set_Y(double cY) { const unsigned int index_1 = 1; cP[index_1] = cY; } //! Set the point cZ coordinate in the camera frame. -void vpPoint::set_Z(double cZ) { cP[2] = cZ; } +void vpPoint::set_Z(double cZ) { const unsigned int index_2 = 2; cP[index_2] = cZ; } //! Set the point cW coordinate in the camera frame. -void vpPoint::set_W(double cW) { cP[3] = cW; } +void vpPoint::set_W(double cW) { const unsigned int index_3 = 3; cP[index_3] = cW; } //! Set the point oX coordinate in the object frame. -void vpPoint::set_oX(double oX) { oP[0] = oX; } +void vpPoint::set_oX(double oX) { const unsigned int index_0 = 0; oP[index_0] = oX; } //! Set the point oY coordinate in the object frame. -void vpPoint::set_oY(double oY) { oP[1] = oY; } +void vpPoint::set_oY(double oY) { const unsigned int index_1 = 1; oP[index_1] = oY; } //! Set the point oZ coordinate in the object frame. -void vpPoint::set_oZ(double oZ) { oP[2] = oZ; } +void vpPoint::set_oZ(double oZ) { const unsigned int index_2 = 2; oP[index_2] = oZ; } //! Set the point oW coordinate in the object frame. -void vpPoint::set_oW(double oW) { oP[3] = oW; } +void vpPoint::set_oW(double oW) { const unsigned int index_3 = 3; oP[index_3] = oW; } //! Set the point x coordinate in the image plane. -void vpPoint::set_x(double x) { p[0] = x; } +void vpPoint::set_x(double x) { const unsigned int index_0 = 0; p[index_0] = x; } //! Set the point y coordinate in the image plane. -void vpPoint::set_y(double y) { p[1] = y; } +void vpPoint::set_y(double y) { const unsigned int index_1 = 1; p[index_1] = y; } //! Set the point w coordinate in the image plane. -void vpPoint::set_w(double w) { p[2] = w; } +void vpPoint::set_w(double w) { const unsigned int index_2 = 2; p[index_2] = w; } VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpPoint & /* vpp */) { return (os << "vpPoint"); } END_VISP_NAMESPACE diff --git a/modules/core/src/tracking/forward-projection/vpSphere.cpp b/modules/core/src/tracking/forward-projection/vpSphere.cpp index 953c3e7c71..4a1aebd246 100644 --- a/modules/core/src/tracking/forward-projection/vpSphere.cpp +++ b/modules/core/src/tracking/forward-projection/vpSphere.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -40,7 +39,7 @@ BEGIN_VISP_NAMESPACE /*! * Initialize internal sphere parameters. */ -void vpSphere::init() + void vpSphere::init() { oP.resize(4); cP.resize(4); @@ -70,10 +69,14 @@ void vpSphere::setWorldCoordinates(const vpColVector &oP_) { this->oP = oP_; } */ void vpSphere::setWorldCoordinates(double oX, double oY, double oZ, double R) { - oP[0] = oX; - oP[1] = oY; - oP[2] = oZ; - oP[3] = R; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + oP[index_0] = oX; + oP[index_1] = oY; + oP[index_2] = oZ; + oP[index_3] = R; } /*! @@ -138,19 +141,24 @@ void vpSphere::projection(const vpColVector &cP_, vpColVector &p_) const p_.resize(5, false); double x0, y0, z0; double E, A, B; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; // calcul des parametres M20, M11, M02 de l'ellipse double s, r; - r = cP_[3]; + r = cP_[index_3]; - x0 = cP_[0]; - y0 = cP_[1]; - z0 = cP_[2]; + x0 = cP_[index_0]; + y0 = cP_[index_1]; + z0 = cP_[index_2]; s = (r * r) - (y0 * y0) - (z0 * z0); if ((s = ((z0 * z0) - (r * r))) < 0.0) { - vpERROR_TRACE("Error: Sphere is behind image plane\n"); + throw(vpException(vpException::fatalError, "Error: Sphere is behind image plane")); } p_[0] = (x0 * z0) / s; // x @@ -161,7 +169,7 @@ void vpSphere::projection(const vpColVector &cP_, vpColVector &p_) const double b = r / sqrt(s); double a = ((x0 * x0) + (y0 * y0) + (z0 * z0)) - (r * r); if (a < 0.0) { - vpERROR_TRACE("Error: Sphere is behind image plane\n"); + throw(vpException(vpException::fatalError, "Error: Sphere is behind image plane")); } a = (r * sqrt(a)) / s; if (fabs(e) <= 1.0) { @@ -187,9 +195,9 @@ void vpSphere::projection(const vpColVector &cP_, vpColVector &p_) const double n11 = ((vpMath::sqr(A) - vpMath::sqr(B)) * E) / det; double n02 = (vpMath::sqr(B) + vpMath::sqr(A * E)) / det; - p_[2] = n20; - p_[3] = n11; - p_[4] = n02; + p_[index_2] = n20; + p_[index_3] = n11; + p_[index_4] = n02; } /*! @@ -211,17 +219,22 @@ void vpSphere::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_) con { cP_.resize(4, false); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + double x0, y0, z0; // variables intermediaires - x0 = (cMo[0][0] * oP[0]) + (cMo[0][1] * oP[1]) + (cMo[0][2] * oP[2]) + cMo[0][3]; - y0 = (cMo[1][0] * oP[0]) + (cMo[1][1] * oP[1]) + (cMo[1][2] * oP[2]) + cMo[1][3]; - z0 = (cMo[2][0] * oP[0]) + (cMo[2][1] * oP[1]) + (cMo[2][2] * oP[2]) + cMo[2][3]; + x0 = (cMo[index_0][0] * oP[0]) + (cMo[index_0][1] * oP[1]) + (cMo[index_0][index_2] * oP[index_2]) + cMo[index_0][index_3]; + y0 = (cMo[index_1][0] * oP[0]) + (cMo[index_1][1] * oP[1]) + (cMo[index_1][index_2] * oP[index_2]) + cMo[index_1][index_3]; + z0 = (cMo[index_2][0] * oP[0]) + (cMo[index_2][1] * oP[1]) + (cMo[index_2][index_2] * oP[index_2]) + cMo[index_2][index_3]; - cP_[3] = oP[3]; + cP_[index_3] = oP[index_3]; - cP_[0] = x0; - cP_[1] = y0; - cP_[2] = z0; + cP_[index_0] = x0; + cP_[index_1] = y0; + cP_[index_2] = z0; } //! For memory issue (used by the vpServo class only). @@ -248,7 +261,12 @@ void vpSphere::display(const vpImage &I, const vpHomogeneousMatri vpColVector v_cP, v_p; changeFrame(cMo, v_cP); projection(v_cP, v_p); - vpFeatureDisplay::displayEllipse(v_p[0], v_p[1], v_p[2], v_p[3], v_p[4], cam, I, color, thickness); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + vpFeatureDisplay::displayEllipse(v_p[index_0], v_p[index_1], v_p[index_2], v_p[index_3], v_p[index_4], cam, I, color, thickness); } /*! @@ -268,7 +286,12 @@ void vpSphere::display(const vpImage &I, const vpHomogeneousMatrix &cMo, vpColVector v_cP, v_p; changeFrame(cMo, v_cP); projection(v_cP, v_p); - vpFeatureDisplay::displayEllipse(v_p[0], v_p[1], v_p[2], v_p[3], v_p[4], cam, I, color, thickness); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + vpFeatureDisplay::displayEllipse(v_p[index_0], v_p[index_1], v_p[index_2], v_p[index_3], v_p[index_4], cam, I, color, thickness); } /*! @@ -282,7 +305,12 @@ void vpSphere::display(const vpImage &I, const vpHomogeneousMatrix &cMo, void vpSphere::display(const vpImage &I, const vpCameraParameters &cam, const vpColor &color, unsigned int thickness) { - vpFeatureDisplay::displayEllipse(p[0], p[1], p[2], p[3], p[4], cam, I, color, thickness); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + vpFeatureDisplay::displayEllipse(p[index_0], p[index_1], p[index_2], p[index_3], p[index_4], cam, I, color, thickness); } /*! @@ -296,6 +324,11 @@ void vpSphere::display(const vpImage &I, const vpCameraParameters void vpSphere::display(const vpImage &I, const vpCameraParameters &cam, const vpColor &color, unsigned int thickness) { - vpFeatureDisplay::displayEllipse(p[0], p[1], p[2], p[3], p[4], cam, I, color, thickness); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + vpFeatureDisplay::displayEllipse(p[index_0], p[index_1], p[index_2], p[index_3], p[index_4], cam, I, color, thickness); } END_VISP_NAMESPACE diff --git a/modules/core/src/tracking/vpTracker.cpp b/modules/core/src/tracking/vpTracker.cpp index 9c2a5374e1..6ef2cd11af 100644 --- a/modules/core/src/tracking/vpTracker.cpp +++ b/modules/core/src/tracking/vpTracker.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -38,7 +37,6 @@ \brief Class that defines what is a generic tracker. */ -#include #include BEGIN_VISP_NAMESPACE diff --git a/modules/core/test/tools/geometry/testImageCircle.cpp b/modules/core/test/tools/geometry/testImageCircle.cpp index 394a79a1fe..9900e0e78e 100644 --- a/modules/core/test/tools/geometry/testImageCircle.cpp +++ b/modules/core/test/tools/geometry/testImageCircle.cpp @@ -62,7 +62,7 @@ int main() { vpImageCircle circle(vpImagePoint(HEIGHT / 2.f, WIDTH / 2.f), RADIUS); float arcLengthCircle = circle.computeArcLengthInRoI(roi); - float theoreticalValue = 2.f * M_PIf * RADIUS; + float theoreticalValue = 2.f * M_PI_FLOAT * RADIUS; bool isValueOK = equal(arcLengthCircle, theoreticalValue); std::string statusTest; if (isValueOK) { @@ -84,7 +84,7 @@ int main() vpRect roiSquare(OFFSET, OFFSET, HEIGHT, HEIGHT); vpImageCircle circle(vpImagePoint(OFFSET + HEIGHT / 2.f, OFFSET + HEIGHT / 2.f), HEIGHT / 2.f); float arcLengthCircle = circle.computeArcLengthInRoI(roi); - float theoreticalValue = 2.f * M_PIf * HEIGHT / 2.f; + float theoreticalValue = 2.f * M_PI_FLOAT * HEIGHT / 2.f; bool isValueOK = equal(arcLengthCircle, theoreticalValue); std::string statusTest; if (isValueOK) { @@ -109,7 +109,7 @@ int main() float vc = OFFSET + 100.f; vpImageCircle circle(vpImagePoint(vc, uc), RADIUS); float arcLengthCircle = circle.computeArcLengthInRoI(roi); - float theoreticalValue = 4.f * M_PIf * RADIUS /3.f; + float theoreticalValue = 4.f * M_PI_FLOAT * RADIUS /3.f; bool isValueOK = equal(arcLengthCircle, theoreticalValue); std::string statusTest; if (isValueOK) { @@ -134,7 +134,7 @@ int main() float vc = OFFSET + 100.f; vpImageCircle circle(vpImagePoint(vc, uc), RADIUS); float arcLengthCircle = circle.computeArcLengthInRoI(roi); - float theoreticalValue = 2.f * M_PIf * RADIUS /3.f; + float theoreticalValue = 2.f * M_PI_FLOAT * RADIUS /3.f; bool isValueOK = equal(arcLengthCircle, theoreticalValue); std::string statusTest; if (isValueOK) { @@ -159,7 +159,7 @@ int main() float vc = OFFSET + 100.f; vpImageCircle circle(vpImagePoint(vc, uc), RADIUS); float arcLengthCircle = circle.computeArcLengthInRoI(roi); - float theoreticalValue = 2.f * M_PIf * RADIUS; + float theoreticalValue = 2.f * M_PI_FLOAT * RADIUS; bool isValueOK = equal(arcLengthCircle, theoreticalValue); std::string statusTest; if (isValueOK) { @@ -209,7 +209,7 @@ int main() float vc = OFFSET + 100.f; vpImageCircle circle(vpImagePoint(vc, uc), RADIUS); float arcLengthCircle = circle.computeArcLengthInRoI(roi); - float theoreticalValue = 4.f * M_PIf * RADIUS /3.f; + float theoreticalValue = 4.f * M_PI_FLOAT * RADIUS /3.f; bool isValueOK = equal(arcLengthCircle, theoreticalValue); std::string statusTest; if (isValueOK) { @@ -234,7 +234,7 @@ int main() float vc = OFFSET + 100.f; vpImageCircle circle(vpImagePoint(vc, uc), RADIUS); float arcLengthCircle = circle.computeArcLengthInRoI(roi); - float theoreticalValue = 2.f * M_PIf * RADIUS /3.f; + float theoreticalValue = 2.f * M_PI_FLOAT * RADIUS /3.f; bool isValueOK = equal(arcLengthCircle, theoreticalValue); std::string statusTest; if (isValueOK) { @@ -259,7 +259,7 @@ int main() float vc = OFFSET + 100.f; vpImageCircle circle(vpImagePoint(vc, uc), RADIUS); float arcLengthCircle = circle.computeArcLengthInRoI(roi); - float theoreticalValue = 2.f * M_PIf * RADIUS; + float theoreticalValue = 2.f * M_PI_FLOAT * RADIUS; bool isValueOK = equal(arcLengthCircle, theoreticalValue); std::string statusTest; if (isValueOK) { @@ -305,12 +305,12 @@ int main() { // v = vc - r sin(theta) // Formula: vc = OFFSET + RADIUS * sin(theta) - float theta = M_PIf / 3.f; + float theta = M_PI_FLOAT / 3.f; float uc = OFFSET + 100.f; float vc = OFFSET + RADIUS * sin(theta); vpImageCircle circle(vpImagePoint(vc, uc), RADIUS); float arcLengthCircle = circle.computeArcLengthInRoI(roi); - float theoreticalValue = 5.f * M_PIf * RADIUS /3.f; + float theoreticalValue = 5.f * M_PI_FLOAT * RADIUS /3.f; bool isValueOK = equal(arcLengthCircle, theoreticalValue); std::string statusTest; if (isValueOK) { @@ -331,12 +331,12 @@ int main() { // v = vc - r sin(theta) // Formula: vc = OFFSET + RADIUS * sin(theta) - float theta = -2.f * M_PIf/3.f; + float theta = -2.f * M_PI_FLOAT/3.f; float uc = OFFSET + 100.f; float vc = OFFSET + RADIUS * std::sin(theta); vpImageCircle circle(vpImagePoint(vc, uc), RADIUS); float arcLengthCircle = circle.computeArcLengthInRoI(roi); - float theoreticalValue = M_PIf * RADIUS /3.f; + float theoreticalValue = M_PI_FLOAT * RADIUS /3.f; bool isValueOK = equal(arcLengthCircle, theoreticalValue); std::string statusTest; if (isValueOK) { @@ -357,12 +357,12 @@ int main() { // v = vc - r sin(theta) // Formula: vc = OFFSET + RADIUS * sin(theta) - float theta = M_PI_2f; + float theta = M_PI_2_FLOAT; float uc = OFFSET + 100.f; float vc = OFFSET + RADIUS * sin(theta); vpImageCircle circle(vpImagePoint(vc, uc), RADIUS); float arcLengthCircle = circle.computeArcLengthInRoI(roi); - float theoreticalValue = 2.f * M_PIf * RADIUS; + float theoreticalValue = 2.f * M_PI_FLOAT * RADIUS; bool isValueOK = equal(arcLengthCircle, theoreticalValue); std::string statusTest; if (isValueOK) { @@ -383,7 +383,7 @@ int main() { // v = vc - r sin(theta) // Formula: vc = OFFSET + RADIUS * sin(theta) - float theta = -M_PI_2f; + float theta = -M_PI_2_FLOAT; float uc = OFFSET + 100.f; float vc = OFFSET + RADIUS * sin(theta); vpImageCircle circle(vpImagePoint(vc, uc), RADIUS); @@ -409,12 +409,12 @@ int main() { // v = vc - r sin(theta) // Formula: vc = OFFSET + HEIGHT + RADIUS * sin(theta) - float theta = -M_PIf / 3.f; + float theta = -M_PI_FLOAT / 3.f; float uc = OFFSET + 100.f; float vc = OFFSET + HEIGHT + RADIUS * std::sin(theta); vpImageCircle circle(vpImagePoint(vc, uc), RADIUS); float arcLengthCircle = circle.computeArcLengthInRoI(roi); - float theoreticalValue = 5.f * M_PIf * RADIUS /3.f; + float theoreticalValue = 5.f * M_PI_FLOAT * RADIUS /3.f; bool isValueOK = equal(arcLengthCircle, theoreticalValue); std::string statusTest; if (isValueOK) { @@ -435,12 +435,12 @@ int main() { // v = vc - r sin(theta) // Formula: vc = OFFSET + HEIGHT + RADIUS * sin(theta) - float theta = M_PIf / 3.f; + float theta = M_PI_FLOAT / 3.f; float uc = OFFSET + 100.f; float vc = OFFSET + HEIGHT + RADIUS * std::sin(theta); vpImageCircle circle(vpImagePoint(vc, uc), RADIUS); float arcLengthCircle = circle.computeArcLengthInRoI(roi); - float theoreticalValue = M_PIf * RADIUS /3.f; + float theoreticalValue = M_PI_FLOAT * RADIUS /3.f; bool isValueOK = equal(arcLengthCircle, theoreticalValue); std::string statusTest; if (isValueOK) { @@ -464,7 +464,7 @@ int main() float vc = OFFSET + HEIGHT - RADIUS; vpImageCircle circle(vpImagePoint(vc, uc), RADIUS); float arcLengthCircle = circle.computeArcLengthInRoI(roi); - float theoreticalValue = 2.f * M_PIf * RADIUS; + float theoreticalValue = 2.f * M_PI_FLOAT * RADIUS; bool isValueOK = equal(arcLengthCircle, theoreticalValue); std::string statusTest; if (isValueOK) { @@ -515,7 +515,7 @@ int main() float vc = OFFSET; vpImageCircle circle(vpImagePoint(vc, uc), RADIUS); float arcLengthCircle = circle.computeArcLengthInRoI(roi); - float theoreticalValue = M_PI_2f * RADIUS; + float theoreticalValue = M_PI_2_FLOAT * RADIUS; bool isValueOK = equal(arcLengthCircle, theoreticalValue); std::string statusTest; if (isValueOK) { @@ -541,13 +541,13 @@ int main() // (4): umin = uc + r cos(theta_v_max) ; v_cross_max = vc - r sin(theta_v_max) >= vmin && <= vmin + height // (3) & (4) => uc = umin - r cos(theta_v_min) = umin - r cos(theta_v_max) <=> theta_v_min = - theta_v_max // (3) & (4) => vc >= vmin + r sin(theta_v_min) && vc >= vmin + r sin (theta_v_max) - float theta_v_min = M_PIf / 4.f; + float theta_v_min = M_PI_FLOAT / 4.f; float uc = OFFSET - RADIUS * std::cos(theta_v_min); float vc = OFFSET + RADIUS * std::sin(theta_v_min) + 1.f; vc = std::max(vc, OFFSET + RADIUS * std::sin(-theta_v_min) + 1.f); vpImageCircle circle(vpImagePoint(vc, uc), RADIUS); float arcLengthCircle = circle.computeArcLengthInRoI(roi); - float theoreticalValue = M_PI_2f * RADIUS; + float theoreticalValue = M_PI_2_FLOAT * RADIUS; bool isValueOK = equal(arcLengthCircle, theoreticalValue); std::string statusTest; if (isValueOK) { @@ -575,13 +575,13 @@ int main() // (1) => uc + r cos(theta_u_top_min) >= umin <=> uc >= umin - r cos(theta_u_top_min) // (2) => uc + r cos(theta_u_top_max) >= umin <=> uc >= umin - r cos(theta_u_top_max) - float theta_u_top_min = -1.1f * M_PI_2f; + float theta_u_top_min = -1.1f * M_PI_2_FLOAT; float uc = OFFSET - RADIUS * std::cos(theta_u_top_min) + 1.f; - uc = std::max(uc, OFFSET - RADIUS * std::cos(M_PIf - theta_u_top_min) + 1.f); + uc = std::max(uc, OFFSET - RADIUS * std::cos(M_PI_FLOAT - theta_u_top_min) + 1.f); float vc = OFFSET + RADIUS * std::sin(theta_u_top_min); vpImageCircle circle(vpImagePoint(vc, uc), RADIUS); float arcLengthCircle = circle.computeArcLengthInRoI(roi); - float theoreticalValue = 0.2f * M_PI_2f * RADIUS; + float theoreticalValue = 0.2f * M_PI_2_FLOAT * RADIUS; bool isValueOK = equal(arcLengthCircle, theoreticalValue); std::string statusTest; if (isValueOK) { @@ -611,10 +611,10 @@ int main() // (3) & (4) =>{ uc = umin - r cos(theta_v_min) & { uc = umin - r cos(- theta_v_min) // (3) & (4) { vc >= vmin - r sin(theta_v_min) & { vc >= vmin - r cos(- theta_v_min) - float theta_u_top_min = 5.f * M_PIf / 8.f; - float theta_u_top_max = M_PIf - theta_u_top_min; + float theta_u_top_min = 5.f * M_PI_FLOAT / 8.f; + float theta_u_top_max = M_PI_FLOAT - theta_u_top_min; float uc = OFFSET - RADIUS * std::cos(theta_u_top_min) + 1.f; - uc = std::max(uc, OFFSET - RADIUS * std::cos(M_PIf - theta_u_top_min) + 1.f); + uc = std::max(uc, OFFSET - RADIUS * std::cos(M_PI_FLOAT - theta_u_top_min) + 1.f); float vc = OFFSET + RADIUS * std::sin(theta_u_top_min); float theta_v_min = std::acos((OFFSET - uc)/RADIUS); theta_v_min = vpMath::getAngleBetweenMinPiAndPi(theta_v_min); @@ -654,13 +654,13 @@ int main() // (1) => vc = vmin + r sin(theta_u_top_min) // (1) & (3) theta_u_top_min = PI - theta_u_top_max // (2) & (4) theta_v_min = - theta_v_max - float theta_u_top_min = 2.f * M_PIf / 3.f; - float theta_v_max = -M_PI_2f; + float theta_u_top_min = 2.f * M_PI_FLOAT / 3.f; + float theta_v_max = -M_PI_2_FLOAT; float uc = OFFSET + WIDTH - RADIUS * std::cos(theta_v_max); float vc = OFFSET + RADIUS * std::sin(theta_u_top_min);; vpImageCircle circle(vpImagePoint(vc, uc), RADIUS); float arcLengthCircle = circle.computeArcLengthInRoI(roi); - float theoreticalValue = (M_PI_2f + M_PIf / 3.f) * RADIUS; + float theoreticalValue = (M_PI_2_FLOAT + M_PI_FLOAT / 3.f) * RADIUS; bool isValueOK = equal(arcLengthCircle, theoreticalValue); std::string statusTest; if (isValueOK) { @@ -689,13 +689,13 @@ int main() // (1) <=> asin((vc - vmin)/r) >= acos[(umin + width - uc)/r] <=> vc >= r sin(acos[(umin + width - uc)/r]) + vmin // (1) & (3) theta_u_top_min = PI - theta_u_top_max // (2) & (4) theta_v_min = - theta_v_max - float theta_v_max = -7.f * M_PIf / 8.f; + float theta_v_max = -7.f * M_PI_FLOAT / 8.f; float theta_v_min = -theta_v_max; float uc = OFFSET + WIDTH - RADIUS * std::cos(theta_v_max); float vc = RADIUS * std::sin(std::acos((OFFSET + WIDTH - uc)/RADIUS)) + OFFSET + 1.f; vpImageCircle circle(vpImagePoint(vc, uc), RADIUS); float arcLengthCircle = circle.computeArcLengthInRoI(roi); - float theoreticalValue = (2.f * M_PIf - (theta_v_min - theta_v_max)) * RADIUS; + float theoreticalValue = (2.f * M_PI_FLOAT - (theta_v_min - theta_v_max)) * RADIUS; bool isValueOK = equal(arcLengthCircle, theoreticalValue); std::string statusTest; if (isValueOK) { @@ -724,8 +724,8 @@ int main() // Choice: theta_u_top_min = -0.9 * PI / 2 // (1) => vc = vmin + r sin(theta_u_top_min) // (2) vc - r sin(theta_v_min) <= vmin => asin((vc - vmin)/r) <= theta_v_min - float theta_u_top_min = -0.9f * M_PI_2f; - float theta_u_top_max = M_PIf - theta_u_top_min; + float theta_u_top_min = -0.9f * M_PI_2_FLOAT; + float theta_u_top_max = M_PI_FLOAT - theta_u_top_min; theta_u_top_max = vpMath::getAngleBetweenMinPiAndPi(theta_u_top_max); float vc = OFFSET + RADIUS * std::sin(theta_u_top_min); float theta_v_min = std::asin((vc - OFFSET)/RADIUS) + 1.f; @@ -762,10 +762,10 @@ int main() // (2) & (4) =>{ uc = umin - r cos(theta_v_min) & { uc = umin - r cos(- theta_v_min) // (2) & (4) { vc >= vmin - r sin(theta_v_min) & { vc >= vmin - r cos(- theta_v_min) - float theta_u_top_min = 5.f * M_PIf / 8.f; - float theta_u_top_max = M_PIf - theta_u_top_min; + float theta_u_top_min = 5.f * M_PI_FLOAT / 8.f; + float theta_u_top_max = M_PI_FLOAT - theta_u_top_min; float uc = OFFSET + WIDTH - RADIUS * std::cos(theta_u_top_min) - 1.f; - uc = std::min(uc, OFFSET + WIDTH - RADIUS * std::cos(M_PIf - theta_u_top_min) - 1.f); + uc = std::min(uc, OFFSET + WIDTH - RADIUS * std::cos(M_PI_FLOAT - theta_u_top_min) - 1.f); float vc = OFFSET + RADIUS * std::sin(theta_u_top_min); float theta_v_min = std::acos((OFFSET + WIDTH - uc)/RADIUS); theta_v_min = vpMath::getAngleBetweenMinPiAndPi(theta_v_min); @@ -777,7 +777,7 @@ int main() } vpImageCircle circle(vpImagePoint(vc, uc), RADIUS); float arcLengthCircle = circle.computeArcLengthInRoI(roi); - float theoreticalValue = (2.f * M_PIf - ((theta_u_top_min - theta_u_top_max) + (theta_v_min - theta_v_max))) * RADIUS; + float theoreticalValue = (2.f * M_PI_FLOAT - ((theta_u_top_min - theta_u_top_max) + (theta_v_min - theta_v_max))) * RADIUS; bool isValueOK = equal(arcLengthCircle, theoreticalValue); std::string statusTest; if (isValueOK) { @@ -805,13 +805,13 @@ int main() // (3) => vc = vmin + height + r sin(theta_u_bot_max) // (1) & (3) theta_u_bot_min = PI - theta_u_bot_max // (2) & (4) theta_v_min = - theta_v_max - float theta_v_min = M_PI_2f; - float theta_u_bot_max = -M_PIf / 3.f; + float theta_v_min = M_PI_2_FLOAT; + float theta_u_bot_max = -M_PI_FLOAT / 3.f; float uc = OFFSET - RADIUS * std::cos(theta_v_min); float vc = OFFSET + HEIGHT + RADIUS * std::sin(theta_u_bot_max);; vpImageCircle circle(vpImagePoint(vc, uc), RADIUS); float arcLengthCircle = circle.computeArcLengthInRoI(roi); - float theoreticalValue = (M_PI_2f + M_PIf / 3.f) * RADIUS; + float theoreticalValue = (M_PI_2_FLOAT + M_PI_FLOAT / 3.f) * RADIUS; bool isValueOK = equal(arcLengthCircle, theoreticalValue); std::string statusTest; if (isValueOK) { @@ -841,7 +841,7 @@ int main() // (4) => vc <= vmin + height + r sin(theta_v_max) // (1) & (3) theta_u_bot_min = PI - theta_u_bot_max // (2) & (4) theta_v_min = - theta_v_max - float theta_v_min = M_PI_4f / 2.f; + float theta_v_min = M_PI_4_FLOAT / 2.f; float theta_v_max = -theta_v_min; float uc = OFFSET - RADIUS * std::cos(theta_v_min); float vc = std::min(OFFSET + HEIGHT + RADIUS * std::sin(theta_v_min) - 1.f, OFFSET + HEIGHT + RADIUS * std::sin(theta_v_max) - 1.f); @@ -877,8 +877,8 @@ int main() // (1) => uc >= umin - r cos(theta_u_bot_max) // (1) & (3) theta_u_bot_min = PI - theta_u_bot_max // (2) & (4) theta_v_min = - theta_v_max - float theta_u_bot_min = 5.f * M_PI_4f / 2.f; - float theta_u_bot_max = M_PIf - theta_u_bot_min; + float theta_u_bot_min = 5.f * M_PI_4_FLOAT / 2.f; + float theta_u_bot_max = M_PI_FLOAT - theta_u_bot_min; float vc = OFFSET + HEIGHT + RADIUS * std::sin(theta_u_bot_min); float uc = std::max(OFFSET - RADIUS * std::cos(theta_u_bot_min) + 1.f, OFFSET - RADIUS * std::cos(theta_u_bot_max) + 1.f); vpImageCircle circle(vpImagePoint(vc, uc), RADIUS); @@ -913,10 +913,10 @@ int main() // (2) & (4) => vc < vmin + height + r sin(theta_v_min) & vc < vmin + height + r sin(-theta_v_min) // (1) & (3) theta_u_bot_min = PI - theta_u_bot_max // (2) & (4) theta_v_min = - theta_v_max - float theta_u_bot_min = -5.f * M_PIf / 8.f; - float theta_u_bot_max = M_PIf - theta_u_bot_min; + float theta_u_bot_min = -5.f * M_PI_FLOAT / 8.f; + float theta_u_bot_max = M_PI_FLOAT - theta_u_bot_min; theta_u_bot_max = vpMath::getAngleBetweenMinPiAndPi(theta_u_bot_max); - float theta_v_min = 7.f * M_PIf / 8.f; + float theta_v_min = 7.f * M_PI_FLOAT / 8.f; float theta_v_max = -theta_v_min; float vc = OFFSET + HEIGHT + RADIUS * std::sin(theta_u_bot_min); float uc = OFFSET - RADIUS * std::cos(theta_v_min); @@ -950,13 +950,13 @@ int main() // (1) => vc = vmin + height + r sin(theta_u_bot_min) // (1) & (3) theta_u_bot_min = PI - theta_u_bot_max // (2) & (4) theta_v_min = - theta_v_max - float theta_u_bot_min = -2.f * M_PIf / 3.f; - float theta_v_min = M_PI_2f; + float theta_u_bot_min = -2.f * M_PI_FLOAT / 3.f; + float theta_v_min = M_PI_2_FLOAT; float uc = OFFSET + WIDTH - RADIUS * std::cos(theta_v_min); float vc = OFFSET + HEIGHT + RADIUS * std::sin(theta_u_bot_min);; vpImageCircle circle(vpImagePoint(vc, uc), RADIUS); float arcLengthCircle = circle.computeArcLengthInRoI(roi); - float theoreticalValue = (M_PI_2f + M_PIf / 3.f) * RADIUS; + float theoreticalValue = (M_PI_2_FLOAT + M_PI_FLOAT / 3.f) * RADIUS; bool isValueOK = equal(arcLengthCircle, theoreticalValue); std::string statusTest; if (isValueOK) { @@ -985,12 +985,12 @@ int main() // (2) & (4) => vc <= vmin + height + r sin(theta_v_min) & vc <= vmin + height + r sin(-theta_v_min) // (1) & (3) theta_u_bot_min = PI - theta_u_bot_max // (2) & (4) theta_v_min = - theta_v_max - float theta_v_min = 5.f * M_PIf / 6.f; + float theta_v_min = 5.f * M_PI_FLOAT / 6.f; float uc = OFFSET + WIDTH - RADIUS * std::cos(theta_v_min); float vc = std::min(OFFSET + HEIGHT + RADIUS * std::sin(theta_v_min) - 1.f, OFFSET + HEIGHT + RADIUS * std::sin(-theta_v_min) - 1.f); vpImageCircle circle(vpImagePoint(vc, uc), RADIUS); float arcLengthCircle = circle.computeArcLengthInRoI(roi); - float theoreticalValue = (M_PIf / 3.f) * RADIUS; // <=> 2.f * M_PIf / 6.f + float theoreticalValue = (M_PI_FLOAT / 3.f) * RADIUS; // <=> 2.f * M_PI_FLOAT / 6.f bool isValueOK = equal(arcLengthCircle, theoreticalValue); std::string statusTest; if (isValueOK) { @@ -1019,12 +1019,12 @@ int main() // (1) & (3) => uc < umin + width - r cos(theta_u_bot_min) & uc <= umin + width - r cos(PI - theta_u_bot_min) // (1) & (3) theta_u_bot_min = PI - theta_u_bot_max // (2) & (4) theta_v_min = - theta_v_max - float theta_u_bot_min = 4.f * M_PIf / 6.f; + float theta_u_bot_min = 4.f * M_PI_FLOAT / 6.f; float vc = OFFSET + HEIGHT + RADIUS * std::sin(theta_u_bot_min); - float uc = std::min(OFFSET + WIDTH - RADIUS * std::cos(theta_u_bot_min) - 1.f, OFFSET + WIDTH - RADIUS * std::cos(M_PIf -theta_u_bot_min) - 1.f); + float uc = std::min(OFFSET + WIDTH - RADIUS * std::cos(theta_u_bot_min) - 1.f, OFFSET + WIDTH - RADIUS * std::cos(M_PI_FLOAT -theta_u_bot_min) - 1.f); vpImageCircle circle(vpImagePoint(vc, uc), RADIUS); float arcLengthCircle = circle.computeArcLengthInRoI(roi); - float theoreticalValue = (M_PIf / 3.f) * RADIUS; // <=> 2.f * M_PIf / 6.f + float theoreticalValue = (M_PI_FLOAT / 3.f) * RADIUS; // <=> 2.f * M_PI_FLOAT / 6.f bool isValueOK = equal(arcLengthCircle, theoreticalValue); std::string statusTest; if (isValueOK) { @@ -1054,16 +1054,16 @@ int main() // (2) & (4) => vc < vmin + height + r sin(theta_v_min) & vc < vmin + height + r sin(-theta_v_min) // (1) & (3) theta_u_bot_min = PI - theta_u_bot_max // (2) & (4) theta_v_min = - theta_v_max - float theta_u_bot_min = -7.f * M_PIf / 8.f; - float theta_u_bot_max = M_PIf - theta_u_bot_min; + float theta_u_bot_min = -7.f * M_PI_FLOAT / 8.f; + float theta_u_bot_max = M_PI_FLOAT - theta_u_bot_min; theta_u_bot_max = vpMath::getAngleBetweenMinPiAndPi(theta_u_bot_max); - float theta_v_max = -3.f * M_PIf / 8.f; + float theta_v_max = -3.f * M_PI_FLOAT / 8.f; float theta_v_min = -theta_v_max; float vc = OFFSET + HEIGHT + RADIUS * std::sin(theta_u_bot_min); float uc = OFFSET - RADIUS * std::cos(theta_v_min); vpImageCircle circle(vpImagePoint(vc, uc), RADIUS); float arcLengthCircle = circle.computeArcLengthInRoI(roi); - float theoreticalValue = (2.f * M_PIf - ((theta_v_min - theta_v_max) + (theta_u_bot_max - theta_u_bot_min))) * RADIUS; + float theoreticalValue = (2.f * M_PI_FLOAT - ((theta_v_min - theta_v_max) + (theta_u_bot_max - theta_u_bot_min))) * RADIUS; bool isValueOK = equal(arcLengthCircle, theoreticalValue); std::string statusTest; if (isValueOK) { @@ -1092,12 +1092,12 @@ int main() // (1) & (3) theta_u_top_min = PI - theta_u_top_max // (2) & (4) theta_v_min = - theta_v_max // (5) & (6) theta_u_bottom_min = PI - theta_u_bottom_max - float theta_u_top_min = 5.f * M_PIf / 8.f; - float theta_u_top_max = 3.f * M_PIf / 8.f; - float theta_v_min = 7.f * M_PIf / 8.f; + float theta_u_top_min = 5.f * M_PI_FLOAT / 8.f; + float theta_u_top_max = 3.f * M_PI_FLOAT / 8.f; + float theta_v_min = 7.f * M_PI_FLOAT / 8.f; float theta_v_max = -theta_v_min; - float theta_u_bottom_min = -5.f * M_PIf / 8.f; - float theta_u_bottom_max = -3.f * M_PIf / 8.f; + float theta_u_bottom_min = -5.f * M_PI_FLOAT / 8.f; + float theta_u_bottom_max = -3.f * M_PI_FLOAT / 8.f; float vc = OFFSET + HEIGHT / 2.f; float radius = -(OFFSET - vc)/ std::sin(theta_u_top_min); float uc = OFFSET - radius * std::cos(theta_v_min); @@ -1132,9 +1132,9 @@ int main() // (1) & (3) theta_u_top_min = PI - theta_u_top_max // (2) & (4) theta_v_min = - theta_v_max // (5) & (6) theta_u_bottom_min = PI - theta_u_bottom_max - float theta_u_top_max = M_PIf / 6.f; - float theta_u_top_min = M_PIf - theta_u_top_max; - float theta_v_min = M_PIf / 3.f; + float theta_u_top_max = M_PI_FLOAT / 6.f; + float theta_u_top_min = M_PI_FLOAT - theta_u_top_max; + float theta_v_min = M_PI_FLOAT / 3.f; float theta_u_bottom_max = -theta_u_top_max; float radius = HEIGHT; float vc = OFFSET + radius * std::sin(theta_u_top_min); @@ -1170,9 +1170,9 @@ int main() // (1) & (3) theta_u_top_min = PI - theta_u_top_max // (2) & (4) theta_v_min = - theta_v_max // (5) & (6) theta_u_bottom_min = PI - theta_u_bottom_max - float theta_u_top_min = 4.f * M_PIf / 6.f; - float theta_u_top_max = M_PIf - theta_u_top_min; - float theta_v_min = M_PIf; + float theta_u_top_min = 4.f * M_PI_FLOAT / 6.f; + float theta_u_top_max = M_PI_FLOAT - theta_u_top_min; + float theta_v_min = M_PI_FLOAT; float theta_u_bottom_min = -theta_u_top_min; float theta_u_bottom_max = -theta_u_top_max; float radius = HEIGHT / (2.f * std::sin(theta_u_top_min)); // vmin + h - vmin = (vc - r sin(-theta_u_top_min)) - (vc - r sin(theta_top_min)) @@ -1180,7 +1180,7 @@ int main() float uc = OFFSET - radius * std::cos(theta_v_min); vpImageCircle circle(vpImagePoint(vc, uc), radius); float arcLengthCircle = circle.computeArcLengthInRoI(roi); - float theoreticalValue = (2.f * M_PIf - ((theta_u_top_min - theta_u_top_max) + (theta_u_bottom_max - theta_u_bottom_min))) * radius; + float theoreticalValue = (2.f * M_PI_FLOAT - ((theta_u_top_min - theta_u_top_max) + (theta_u_bottom_max - theta_u_bottom_min))) * radius; bool isValueOK = equal(arcLengthCircle, theoreticalValue); std::string statusTest; if (isValueOK) { @@ -1209,8 +1209,8 @@ int main() // (1) & (3) theta_u_top_min = PI - theta_u_top_max // (2) & (4) theta_v_min = - theta_v_max // (5) & (6) theta_u_bottom_min = PI - theta_u_bottom_max - float theta_u_top_min = M_PI_2f; - float theta_v_min = M_PI_4f; + float theta_u_top_min = M_PI_2_FLOAT; + float theta_v_min = M_PI_4_FLOAT; float theta_v_max = -theta_v_min; float radius = HEIGHT / 2.f; float vc = OFFSET + radius * std::sin(theta_u_top_min); @@ -1246,8 +1246,8 @@ int main() // (1) & (3) theta_u_top_min = PI - theta_u_top_max // (2) & (4) theta_v_min = - theta_v_max // (5) & (6) theta_u_bottom_min = PI - theta_u_bottom_max - float theta_u_top_min = M_PI_2f; - float theta_v_min = 3.f * M_PI_4f; + float theta_u_top_min = M_PI_2_FLOAT; + float theta_v_min = 3.f * M_PI_4_FLOAT; float theta_v_max = -theta_v_min; float radius = HEIGHT / 2.f; float vc = OFFSET + radius * std::sin(theta_u_top_min); @@ -1284,8 +1284,8 @@ int main() // (2) & (4) theta_v_min = - theta_v_max // (5) & (6) theta_u_bottom_min = PI - theta_u_bottom_max float theta_u_top_max = 0.f; - float theta_u_bot_max = -M_PIf / 3.f; - float theta_v_max = -M_PIf / 6.f; + float theta_u_bot_max = -M_PI_FLOAT / 3.f; + float theta_v_max = -M_PI_FLOAT / 6.f; float radius = HEIGHT / (std::sin(theta_u_top_max) - std::sin(theta_u_bot_max)); float uc = OFFSET - radius * std::cos(theta_v_max); float vc = OFFSET + radius * std::sin(theta_u_top_max); @@ -1320,9 +1320,9 @@ int main() // (1) & (3) theta_u_top_min = PI - theta_u_top_max // (2) & (4) theta_v_min = - theta_v_max // (5) & (6) theta_u_bottom_min = PI - theta_u_bottom_max - float theta_u_top_max = M_PIf / 3.f; + float theta_u_top_max = M_PI_FLOAT / 3.f; float theta_u_bot_max = 0.f; - float theta_v_min = M_PIf / 6.f; + float theta_v_min = M_PI_FLOAT / 6.f; float radius = HEIGHT / (std::sin(theta_u_top_max) - std::sin(theta_u_bot_max)); float uc = OFFSET - radius * std::cos(theta_v_min); float vc = OFFSET + radius * std::sin(theta_u_top_max); @@ -1357,18 +1357,18 @@ int main() // (1) & (3) theta_u_top_min = PI - theta_u_top_max // (2) & (4) theta_v_min = - theta_v_max // (5) & (6) theta_u_bottom_min = PI - theta_u_bottom_max - float theta_u_top_min = 5.f * M_PIf / 8.f; - float theta_u_top_max = 3.f * M_PIf / 8.f; - float theta_v_min = 1.f * M_PIf / 8.f; + float theta_u_top_min = 5.f * M_PI_FLOAT / 8.f; + float theta_u_top_max = 3.f * M_PI_FLOAT / 8.f; + float theta_v_min = 1.f * M_PI_FLOAT / 8.f; float theta_v_max = -theta_v_min; - float theta_u_bottom_min = -5.f * M_PIf / 8.f; - float theta_u_bottom_max = -3.f * M_PIf / 8.f; + float theta_u_bottom_min = -5.f * M_PI_FLOAT / 8.f; + float theta_u_bottom_max = -3.f * M_PI_FLOAT / 8.f; float vc = OFFSET + HEIGHT / 2.f; float radius = -(OFFSET - vc)/ std::sin(theta_u_top_min); float uc = OFFSET + WIDTH - radius * std::cos(theta_v_min); vpImageCircle circle(vpImagePoint(vc, uc), radius); float arcLengthCircle = circle.computeArcLengthInRoI(roi); - float theoreticalValue = (2.f * M_PIf - ((theta_u_top_min - theta_u_top_max) + (theta_v_min - theta_v_max) + (theta_u_bottom_max - theta_u_bottom_min))) * radius; + float theoreticalValue = (2.f * M_PI_FLOAT - ((theta_u_top_min - theta_u_top_max) + (theta_v_min - theta_v_max) + (theta_u_bottom_max - theta_u_bottom_min))) * radius; bool isValueOK = equal(arcLengthCircle, theoreticalValue); std::string statusTest; if (isValueOK) { @@ -1397,15 +1397,15 @@ int main() // (1) & (3) theta_u_top_min = PI - theta_u_top_max // (2) & (4) theta_v_min = - theta_v_max // (5) & (6) theta_u_bottom_min = PI - theta_u_bottom_max - float theta_u_top_min = 5.f * M_PIf / 6.f; - float theta_v_min = 2.f * M_PIf / 3.f; + float theta_u_top_min = 5.f * M_PI_FLOAT / 6.f; + float theta_v_min = 2.f * M_PI_FLOAT / 3.f; float theta_u_bottom_min = -theta_u_top_min; float radius = HEIGHT; float vc = OFFSET + radius * std::sin(theta_u_top_min); float uc = OFFSET + WIDTH - radius * std::cos(theta_v_min); vpImageCircle circle(vpImagePoint(vc, uc), radius); float arcLengthCircle = circle.computeArcLengthInRoI(roi); - float theoreticalValue = (2.f * M_PIf - (theta_u_top_min - theta_u_bottom_min)) * radius; + float theoreticalValue = (2.f * M_PI_FLOAT - (theta_u_top_min - theta_u_bottom_min)) * radius; bool isValueOK = equal(arcLengthCircle, theoreticalValue); std::string statusTest; if (isValueOK) { @@ -1434,8 +1434,8 @@ int main() // (1) & (3) theta_u_top_min = PI - theta_u_top_max // (2) & (4) theta_v_min = - theta_v_max // (5) & (6) theta_u_bottom_min = PI - theta_u_bottom_max - float theta_u_top_min = 4.f * M_PIf / 6.f; - float theta_u_top_max = M_PIf - theta_u_top_min; + float theta_u_top_min = 4.f * M_PI_FLOAT / 6.f; + float theta_u_top_max = M_PI_FLOAT - theta_u_top_min; float theta_v_min = 0; float theta_u_bottom_min = -theta_u_top_min; float theta_u_bottom_max = -theta_u_top_max; @@ -1444,7 +1444,7 @@ int main() float uc = OFFSET + WIDTH - radius * std::cos(theta_v_min); vpImageCircle circle(vpImagePoint(vc, uc), radius); float arcLengthCircle = circle.computeArcLengthInRoI(roi); - float theoreticalValue = (2.f * M_PIf - ((theta_u_top_min - theta_u_top_max) + (theta_u_bottom_max - theta_u_bottom_min))) * radius; + float theoreticalValue = (2.f * M_PI_FLOAT - ((theta_u_top_min - theta_u_top_max) + (theta_u_bottom_max - theta_u_bottom_min))) * radius; bool isValueOK = equal(arcLengthCircle, theoreticalValue); std::string statusTest; if (isValueOK) { @@ -1473,15 +1473,15 @@ int main() // (1) & (3) theta_u_top_min = PI - theta_u_top_max // (2) & (4) theta_v_min = - theta_v_max // (5) & (6) theta_u_bottom_min = PI - theta_u_bottom_max - float theta_u_top_min = M_PI_2f; - float theta_v_min = 3.f * M_PI_4f; + float theta_u_top_min = M_PI_2_FLOAT; + float theta_v_min = 3.f * M_PI_4_FLOAT; float theta_v_max = -theta_v_min; float radius = HEIGHT / 2.f; float vc = OFFSET + radius * std::sin(theta_u_top_min); float uc = OFFSET + WIDTH - radius * std::cos(theta_v_min); vpImageCircle circle(vpImagePoint(vc, uc), radius); float arcLengthCircle = circle.computeArcLengthInRoI(roi); - float theoreticalValue = (2.f * M_PIf - (theta_v_min - theta_v_max)) * radius; + float theoreticalValue = (2.f * M_PI_FLOAT - (theta_v_min - theta_v_max)) * radius; bool isValueOK = equal(arcLengthCircle, theoreticalValue); std::string statusTest; if (isValueOK) { @@ -1510,15 +1510,15 @@ int main() // (1) & (3) theta_u_top_min = PI - theta_u_top_max // (2) & (4) theta_v_min = - theta_v_max // (5) & (6) theta_u_bottom_min = PI - theta_u_bottom_max - float theta_u_top_min = M_PI_2f; - float theta_v_min = M_PI_4f; + float theta_u_top_min = M_PI_2_FLOAT; + float theta_v_min = M_PI_4_FLOAT; float theta_v_max = -theta_v_min; float radius = HEIGHT / 2.f; float vc = OFFSET + radius * std::sin(theta_u_top_min); float uc = OFFSET + WIDTH - radius * std::cos(theta_v_min); vpImageCircle circle(vpImagePoint(vc, uc), radius); float arcLengthCircle = circle.computeArcLengthInRoI(roi); - float theoreticalValue = (2.f * M_PIf - (theta_v_min - theta_v_max)) * radius; + float theoreticalValue = (2.f * M_PI_FLOAT - (theta_v_min - theta_v_max)) * radius; bool isValueOK = equal(arcLengthCircle, theoreticalValue); std::string statusTest; if (isValueOK) { @@ -1547,15 +1547,15 @@ int main() // (1) & (3) theta_u_top_min = PI - theta_u_top_max // (2) & (4) theta_v_min = - theta_v_max // (5) & (6) theta_u_bottom_min = PI - theta_u_bottom_max - float theta_u_top_min = M_PIf; - float theta_u_bot_min = -2.f * M_PIf / 3.f; - float theta_v_max = -5.f * M_PIf / 6.f; + float theta_u_top_min = M_PI_FLOAT; + float theta_u_bot_min = -2.f * M_PI_FLOAT / 3.f; + float theta_v_max = -5.f * M_PI_FLOAT / 6.f; float radius = HEIGHT / (std::sin(theta_u_top_min) - std::sin(theta_u_bot_min)); float uc = OFFSET + WIDTH - radius * std::cos(theta_v_max); float vc = OFFSET + radius * std::sin(theta_u_top_min); vpImageCircle circle(vpImagePoint(vc, uc), radius); float arcLengthCircle = circle.computeArcLengthInRoI(roi); - float theoreticalValue = (2.f * M_PIf - (theta_u_top_min - theta_v_max)) * radius; + float theoreticalValue = (2.f * M_PI_FLOAT - (theta_u_top_min - theta_v_max)) * radius; bool isValueOK = equal(arcLengthCircle, theoreticalValue); std::string statusTest; if (isValueOK) { @@ -1584,9 +1584,9 @@ int main() // (1) & (3) theta_u_top_min = PI - theta_u_top_max // (2) & (4) theta_v_min = - theta_v_max // (5) & (6) theta_u_bottom_min = PI - theta_u_bottom_max - float theta_u_top_min = 2.f * M_PIf / 3.f; - float theta_u_bot_min = M_PIf; - float theta_v_min = 5.f * M_PIf / 6.f; + float theta_u_top_min = 2.f * M_PI_FLOAT / 3.f; + float theta_u_bot_min = M_PI_FLOAT; + float theta_v_min = 5.f * M_PI_FLOAT / 6.f; float radius = HEIGHT / (std::sin(theta_u_top_min) - std::sin(theta_u_bot_min)); float uc = OFFSET + WIDTH - radius * std::cos(theta_v_min); float vc = OFFSET + radius * std::sin(theta_u_top_min); @@ -1623,12 +1623,12 @@ int main() // (2) & (4) theta_v_left_min = - theta_v_left_max // (5) & (6) theta_v_right_min = - theta_v_right_max - float theta_v_left_min = 7.f * M_PIf / 8.f; + float theta_v_left_min = 7.f * M_PI_FLOAT / 8.f; float theta_v_left_max = -theta_v_left_min; - float theta_v_right_min = M_PIf / 8.f; + float theta_v_right_min = M_PI_FLOAT / 8.f; float theta_v_right_max = -theta_v_right_min; - float theta_u_top_min = 5.f * M_PIf / 8.f; - float theta_u_top_max = M_PIf - theta_u_top_min; + float theta_u_top_min = 5.f * M_PI_FLOAT / 8.f; + float theta_u_top_max = M_PI_FLOAT - theta_u_top_min; float radius = WIDTH_SWITCHED / (std::cos(theta_v_right_min) - std::cos(theta_v_left_min)); float uc = OFFSET + WIDTH_SWITCHED - radius * std::cos(theta_v_right_min); float vc = OFFSET + radius * std::sin(theta_u_top_min); @@ -1664,12 +1664,12 @@ int main() // (2) & (4) theta_v_left_min = - theta_v_left_max // (5) & (6) theta_v_right_min = - theta_v_right_max - float theta_u_top_min = -2.f * M_PIf / 3.f; + float theta_u_top_min = -2.f * M_PI_FLOAT / 3.f; float uc = OFFSET + WIDTH_SWITCHED/2.f; float vc = OFFSET + RADIUS * std::sin(theta_u_top_min); vpImageCircle circle(vpImagePoint(vc, uc), RADIUS); float arcLengthCircle = circle.computeArcLengthInRoI(switchedRoI); - float theoreticalValue = (M_PIf/3.f) * RADIUS; + float theoreticalValue = (M_PI_FLOAT/3.f) * RADIUS; bool isValueOK = equal(arcLengthCircle, theoreticalValue); std::string statusTest; if (isValueOK) { @@ -1700,9 +1700,9 @@ int main() // (2) & (4) theta_v_left_min = - theta_v_left_max // (5) & (6) theta_v_right_min = - theta_v_right_max - float theta_v_left_max = -5.f * M_PIf / 8.f; - float theta_v_right_max = -3.f *M_PIf / 8.f; - float theta_u_top_min = -7.f * M_PIf / 8.f; + float theta_v_left_max = -5.f * M_PI_FLOAT / 8.f; + float theta_v_right_max = -3.f *M_PI_FLOAT / 8.f; + float theta_u_top_min = -7.f * M_PI_FLOAT / 8.f; float radius = WIDTH_SWITCHED / (std::cos(theta_v_right_max) - std::cos(theta_v_left_max)); float uc = OFFSET - radius * std::cos(theta_v_left_max); float vc = OFFSET + radius * std::sin(theta_u_top_min); @@ -1739,9 +1739,9 @@ int main() // (2) & (4) theta_v_left_min = - theta_v_left_max // (5) & (6) theta_v_right_min = - theta_v_right_max - float theta_u_top_max = -M_PIf / 3.f; + float theta_u_top_max = -M_PI_FLOAT / 3.f; float theta_v_right_max = 0.f; - float theta_v_left_max = -M_PI_2f; + float theta_v_left_max = -M_PI_2_FLOAT; float radius = WIDTH_SWITCHED / (std::cos(theta_v_right_max) - std::cos(theta_v_left_max)); float uc = OFFSET; float vc = OFFSET + radius * std::sin(theta_u_top_max); @@ -1778,9 +1778,9 @@ int main() // (2) & (4) theta_v_left_min = - theta_v_left_max // (5) & (6) theta_v_right_min = - theta_v_right_max - float theta_u_top_min = -2.f * M_PIf / 3.f; - float theta_v_left_max = M_PIf; - float theta_v_right_max = -M_PI_2f; + float theta_u_top_min = -2.f * M_PI_FLOAT / 3.f; + float theta_v_left_max = M_PI_FLOAT; + float theta_v_right_max = -M_PI_2_FLOAT; float radius = WIDTH_SWITCHED / (std::cos(theta_v_right_max) - std::cos(theta_v_left_max)); float uc = OFFSET + WIDTH_SWITCHED; float vc = OFFSET + radius * std::sin(theta_u_top_min); @@ -1817,12 +1817,12 @@ int main() // (2) & (4) theta_v_left_min = - theta_v_left_max // (5) & (6) theta_v_right_min = - theta_v_right_max - float theta_v_left_min = 7.f * M_PIf / 8.f; + float theta_v_left_min = 7.f * M_PI_FLOAT / 8.f; float theta_v_left_max = -theta_v_left_min; - float theta_v_right_min = M_PIf / 8.f; + float theta_v_right_min = M_PI_FLOAT / 8.f; float theta_v_right_max = -theta_v_right_min; - float theta_u_bot_min = -5.f * M_PIf / 8.f; - float theta_u_bot_max = -M_PIf - theta_u_bot_min; + float theta_u_bot_min = -5.f * M_PI_FLOAT / 8.f; + float theta_u_bot_max = -M_PI_FLOAT - theta_u_bot_min; float radius = WIDTH_SWITCHED / (std::cos(theta_v_right_min) - std::cos(theta_v_left_min)); float uc = OFFSET + WIDTH_SWITCHED - radius * std::cos(theta_v_right_min); float vc = OFFSET + HEIGHT_SWITCHED + radius * std::sin(theta_u_bot_min); @@ -1858,10 +1858,10 @@ int main() // (1) & (3) theta_u_top_min = PI - theta_u_top_max // (2) & (4) theta_v_left_min = - theta_v_left_max // (5) & (6) theta_v_right_min = - theta_v_right_max - float theta_u_bot_min = 2.f * M_PIf / 3.f; - float theta_u_bot_max = M_PIf - theta_u_bot_min; - float theta_v_left_min = 5.f * M_PIf / 6.f; - float theta_v_right_min = M_PIf / 6.f; + float theta_u_bot_min = 2.f * M_PI_FLOAT / 3.f; + float theta_u_bot_max = M_PI_FLOAT - theta_u_bot_min; + float theta_v_left_min = 5.f * M_PI_FLOAT / 6.f; + float theta_v_right_min = M_PI_FLOAT / 6.f; float radius = WIDTH_SWITCHED / (std::cos(theta_v_right_min) - std::cos(theta_v_left_min)); float uc = OFFSET + WIDTH_SWITCHED - radius * std::cos(theta_v_right_min); float vc = OFFSET + HEIGHT_SWITCHED + radius * std::sin(theta_u_bot_min); @@ -1897,9 +1897,9 @@ int main() // (1) & (3) theta_u_top_min = PI - theta_u_top_max // (2) & (4) theta_v_left_min = - theta_v_left_max // (5) & (6) theta_v_right_min = - theta_v_right_max - float theta_u_bot_min = 7.f * M_PIf / 8.f; - float theta_v_left_min = 5.f * M_PIf / 8.f; - float theta_v_right_min = 3.f * M_PIf / 8.f; + float theta_u_bot_min = 7.f * M_PI_FLOAT / 8.f; + float theta_v_left_min = 5.f * M_PI_FLOAT / 8.f; + float theta_v_right_min = 3.f * M_PI_FLOAT / 8.f; float radius = WIDTH_SWITCHED / (std::cos(theta_v_right_min) - std::cos(theta_v_left_min)); float uc = OFFSET + WIDTH_SWITCHED - radius * std::cos(theta_v_right_min); float vc = OFFSET + HEIGHT_SWITCHED + radius * std::sin(theta_u_bot_min); @@ -1935,8 +1935,8 @@ int main() // (1) & (3) theta_u_top_min = PI - theta_u_top_max // (2) & (4) theta_v_left_min = - theta_v_left_max // (5) & (6) theta_v_right_min = - theta_v_right_max - float theta_u_bot_max = M_PIf / 3.f; - float theta_v_left_min = M_PI_2f; + float theta_u_bot_max = M_PI_FLOAT / 3.f; + float theta_v_left_min = M_PI_2_FLOAT; float theta_v_right_min = 0.f; float radius = WIDTH_SWITCHED / (std::cos(theta_v_right_min) - std::cos(theta_v_left_min)); float uc = OFFSET; @@ -1973,9 +1973,9 @@ int main() // (1) & (3) theta_u_top_min = PI - theta_u_top_max // (2) & (4) theta_v_left_min = - theta_v_left_max // (5) & (6) theta_v_right_min = - theta_v_right_max - float theta_u_bot_min = 2.f * M_PIf / 3.f; - float theta_v_right_min = M_PI_2f; - float theta_v_left_min = M_PIf; + float theta_u_bot_min = 2.f * M_PI_FLOAT / 3.f; + float theta_v_right_min = M_PI_2_FLOAT; + float theta_v_left_min = M_PI_FLOAT; float radius = WIDTH_SWITCHED / (std::cos(theta_v_right_min) - std::cos(theta_v_left_min)); float uc = OFFSET + WIDTH_SWITCHED; float vc = OFFSET + HEIGHT_SWITCHED + radius * std::sin(theta_u_bot_min); @@ -2007,17 +2007,17 @@ int main() // (6): u_cross_bot_max = uc + r cos(theta_u_bottom_max) <= umin_roi + width ; vmin_roi + height = vc - r sin(theta_u_bottom_max) // (1) & (3) theta_u_top_min = PI - theta_u_top_max // (5) & (6) theta_u_bottom_min = PI - theta_u_bottom_max - float theta_u_top_min = 2.f * M_PIf / 3.f; - float theta_u_top_max = M_PIf / 3.f; - float theta_u_bottom_min = -2.f * M_PIf / 3.f; - float theta_u_bottom_max = -M_PIf / 3.f; + float theta_u_top_min = 2.f * M_PI_FLOAT / 3.f; + float theta_u_top_max = M_PI_FLOAT / 3.f; + float theta_u_bottom_min = -2.f * M_PI_FLOAT / 3.f; + float theta_u_bottom_max = -M_PI_FLOAT / 3.f; float uc = OFFSET + WIDTH / 2.f; float vc = OFFSET + HEIGHT / 2.f; float radius = -(OFFSET - vc)/ std::sin(theta_u_top_min); vpImageCircle circle(vpImagePoint(vc, uc), radius); float arcLengthCircle = circle.computeArcLengthInRoI(roi); - float theoreticalValue = (2.f * M_PIf - ((theta_u_top_min - theta_u_top_max) + (theta_u_bottom_max - theta_u_bottom_min))) * radius; + float theoreticalValue = (2.f * M_PI_FLOAT - ((theta_u_top_min - theta_u_top_max) + (theta_u_bottom_max - theta_u_bottom_min))) * radius; bool isValueOK = equal(arcLengthCircle, theoreticalValue); std::string statusTest; if (isValueOK) { @@ -2043,9 +2043,9 @@ int main() // (6): u_min + width = uc + r cos(theta_v_right_max); v_cross_right_max = vc - r sin(theta_v_right_max) // (2) & (4) theta_v_left_min = - theta_v_left_max // (5) & (6) theta_v_right_min = - theta_v_right_max - float theta_v_left_min = 5.f * M_PIf / 6.f; + float theta_v_left_min = 5.f * M_PI_FLOAT / 6.f; float theta_v_left_max = -theta_v_left_min; - float theta_v_right_min = M_PIf / 6.f; + float theta_v_right_min = M_PI_FLOAT / 6.f; float theta_v_right_max = -theta_v_right_min; float uc = OFFSET + HEIGHT / 2.f; float vc = OFFSET + WIDTH / 2.f; @@ -2075,14 +2075,14 @@ int main() // Choosing theta_v_left_min = 7 PI / 8 and circle at the center of the RoI // umin = uc + r cos(theta_v_left_min) => r = (umin - uc) / cos(theta_v_left_min) vpRect squareRoI(OFFSET, OFFSET, HEIGHT, HEIGHT); - float theta_v_left_min = 7.f * M_PIf / 8.f; + float theta_v_left_min = 7.f * M_PI_FLOAT / 8.f; float uc = OFFSET + HEIGHT / 2.f; float vc = OFFSET + HEIGHT / 2.f; float radius = (OFFSET - uc) / std::cos(theta_v_left_min); vpImageCircle circle(vpImagePoint(vc, uc), radius); float arcLengthCircle = circle.computeArcLengthInRoI(squareRoI); - float theoreticalValue = M_PIf * radius; + float theoreticalValue = M_PI_FLOAT * radius; bool isValueOK = equal(arcLengthCircle, theoreticalValue); std::string statusTest; if (isValueOK) { diff --git a/modules/detection/include/visp3/detection/vpDetectorAprilTag.h b/modules/detection/include/visp3/detection/vpDetectorAprilTag.h index 929c6563be..7b8d4411e5 100644 --- a/modules/detection/include/visp3/detection/vpDetectorAprilTag.h +++ b/modules/detection/include/visp3/detection/vpDetectorAprilTag.h @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,10 +29,9 @@ * * Description: * Base class for AprilTag detection. - * -*****************************************************************************/ -#ifndef _vpDetectorAprilTag_h_ -#define _vpDetectorAprilTag_h_ + */ +#ifndef VP_DETECTOR_APRILTAG_H +#define VP_DETECTOR_APRILTAG_H #include @@ -46,13 +44,6 @@ #include #include -BEGIN_VISP_NAMESPACE -class vpDetectorAprilTag; -END_VISP_NAMESPACE - -// Forward declaration to have the operator in the global namespace -VISP_EXPORT void swap(VISP_NAMESPACE_ADDRESSING vpDetectorAprilTag &o1, VISP_NAMESPACE_ADDRESSING vpDetectorAprilTag &o2); - BEGIN_VISP_NAMESPACE /*! * \class vpDetectorAprilTag @@ -220,8 +211,8 @@ BEGIN_VISP_NAMESPACE * * Other examples are also provided in tutorial-apriltag-detector.cpp and * tutorial-apriltag-detector-live.cpp - */ - class VISP_EXPORT vpDetectorAprilTag : public vpDetectorBase +*/ +class VISP_EXPORT vpDetectorAprilTag : public vpDetectorBase { public: enum vpAprilTagFamily @@ -308,7 +299,11 @@ BEGIN_VISP_NAMESPACE m_displayTagThickness = thickness; } - VISP_EXPORT friend void ::swap(vpDetectorAprilTag &o1, vpDetectorAprilTag &o2); + inline friend VISP_EXPORT void swap(vpDetectorAprilTag &o1, vpDetectorAprilTag &o2) + { + using std::swap; + swap(o1.m_impl, o2.m_impl); + } void setZAlignedWithCameraAxis(bool zAlignedWithCameraFrame); diff --git a/modules/detection/include/visp3/detection/vpDetectorBase.h b/modules/detection/include/visp3/detection/vpDetectorBase.h index 2fcdf054d1..436628a11d 100644 --- a/modules/detection/include/visp3/detection/vpDetectorBase.h +++ b/modules/detection/include/visp3/detection/vpDetectorBase.h @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,10 @@ * * Description: * Base class for object detection. - * -*****************************************************************************/ + */ -#ifndef _vpDetectorBase_h_ -#define _vpDetectorBase_h_ +#ifndef VP_DETECTOR_BASE_H +#define VP_DETECTOR_BASE_H #include #include @@ -60,15 +58,9 @@ BEGIN_VISP_NAMESPACE * acquired by a camera. * - AprilTags using vpDetectorAprilTag class * - faces using vpDetectorFace. An example is provided in tutorial-face-detector-live.cpp. - */ +*/ class VISP_EXPORT vpDetectorBase { -protected: - std::vector > m_polygon; //!< For each object, defines the polygon that contains the object. - std::vector m_message; //!< Message attached to each object. - size_t m_nb_objects; //!< Number of detected objects. - unsigned long m_timeout_ms; //!< Detection timeout. - public: /*! * Default constructor. @@ -131,6 +123,12 @@ class VISP_EXPORT vpDetectorBase inline void setTimeout(unsigned long timeout_ms) { m_timeout_ms = timeout_ms; } //@} + +protected: + std::vector > m_polygon; //!< For each object, defines the polygon that contains the object. + std::vector m_message; //!< Message attached to each object. + size_t m_nb_objects; //!< Number of detected objects. + unsigned long m_timeout_ms; //!< Detection timeout. }; END_VISP_NAMESPACE diff --git a/modules/detection/src/tag/vpDetectorAprilTag.cpp b/modules/detection/src/tag/vpDetectorAprilTag.cpp index 08581ad5de..7d9bffd1c8 100644 --- a/modules/detection/src/tag/vpDetectorAprilTag.cpp +++ b/modules/detection/src/tag/vpDetectorAprilTag.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -280,11 +279,12 @@ class vpDetectorAprilTag::Impl void convertHomogeneousMatrix(const apriltag_pose_t &pose, vpHomogeneousMatrix &cMo) { - for (unsigned int i = 0; i < 3; ++i) { - for (unsigned int j = 0; j < 3; ++j) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { + for (unsigned int j = 0; j < val_3; ++j) { cMo[i][j] = MATD_EL(pose.R, i, j); } - cMo[i][3] = MATD_EL(pose.t, i, 0); + cMo[i][val_3] = MATD_EL(pose.t, i, 0); } } @@ -334,7 +334,8 @@ class vpDetectorAprilTag::Impl zarray_get(m_detections, i, &det); std::vector polygon; - for (int j = 0; j < 4; ++j) { + const int polygonSize = 4; + for (int j = 0; j < polygonSize; ++j) { polygon.push_back(vpImagePoint(det->p[j][1], det->p[j][0])); } polygons[static_cast(i)] = polygon; @@ -349,13 +350,14 @@ class vpDetectorAprilTag::Impl vpColor Ox2 = (color == vpColor::none) ? vpColor::yellow : color; vpColor Oy2 = (color == vpColor::none) ? vpColor::blue : color; - vpDisplay::displayLine(I, static_cast(det->p[0][1]), static_cast(det->p[0][0]), static_cast(det->p[1][1]), static_cast(det->p[1][0]), Ox, + const unsigned int polyId0 = 0, polyId1 = 1, polyId2 = 2, polyId3 = 3; + vpDisplay::displayLine(I, static_cast(det->p[polyId0][1]), static_cast(det->p[polyId0][0]), static_cast(det->p[polyId1][1]), static_cast(det->p[polyId1][0]), Ox, thickness); - vpDisplay::displayLine(I, static_cast(det->p[0][1]), static_cast(det->p[0][0]), static_cast(det->p[3][1]), static_cast(det->p[3][0]), Oy, + vpDisplay::displayLine(I, static_cast(det->p[polyId0][1]), static_cast(det->p[polyId0][0]), static_cast(det->p[polyId3][1]), static_cast(det->p[polyId3][0]), Oy, thickness); - vpDisplay::displayLine(I, static_cast(det->p[1][1]), static_cast(det->p[1][0]), static_cast(det->p[2][1]), static_cast(det->p[2][0]), Ox2, + vpDisplay::displayLine(I, static_cast(det->p[polyId1][1]), static_cast(det->p[polyId1][0]), static_cast(det->p[polyId2][1]), static_cast(det->p[polyId2][0]), Ox2, thickness); - vpDisplay::displayLine(I, static_cast(det->p[2][1]), static_cast(det->p[2][0]), static_cast(det->p[3][1]), static_cast(det->p[3][0]), Oy2, + vpDisplay::displayLine(I, static_cast(det->p[polyId2][1]), static_cast(det->p[polyId2][0]), static_cast(det->p[polyId3][1]), static_cast(det->p[polyId3][0]), Oy2, thickness); } @@ -414,14 +416,15 @@ class vpDetectorAprilTag::Impl const std::vector &corners = tagsCorners[i]; assert(corners.size() == 4); + const unsigned int cornerId0 = 0, cornerId1 = 1, cornerId2 = 2, cornerId3 = 3; - vpDisplay::displayLine(I, static_cast(corners[0].get_i()), static_cast(corners[0].get_j()), static_cast(corners[1].get_i()), static_cast(corners[1].get_j()), + vpDisplay::displayLine(I, static_cast(corners[cornerId0].get_i()), static_cast(corners[cornerId0].get_j()), static_cast(corners[cornerId1].get_i()), static_cast(corners[cornerId1].get_j()), Ox, thickness); - vpDisplay::displayLine(I, static_cast(corners[0].get_i()), static_cast(corners[0].get_j()), static_cast(corners[3].get_i()), static_cast(corners[3].get_j()), + vpDisplay::displayLine(I, static_cast(corners[cornerId0].get_i()), static_cast(corners[cornerId0].get_j()), static_cast(corners[cornerId3].get_i()), static_cast(corners[cornerId3].get_j()), Oy, thickness); - vpDisplay::displayLine(I, static_cast(corners[1].get_i()), static_cast(corners[1].get_j()), static_cast(corners[2].get_i()), static_cast(corners[2].get_j()), + vpDisplay::displayLine(I, static_cast(corners[cornerId1].get_i()), static_cast(corners[cornerId1].get_j()), static_cast(corners[cornerId2].get_i()), static_cast(corners[cornerId2].get_j()), Ox2, thickness); - vpDisplay::displayLine(I, static_cast(corners[2].get_i()), static_cast(corners[2].get_j()), static_cast(corners[3].get_i()), static_cast(corners[3].get_j()), + vpDisplay::displayLine(I, static_cast(corners[cornerId2].get_i()), static_cast(corners[cornerId2].get_j()), static_cast(corners[cornerId3].get_i()), static_cast(corners[cornerId3].get_j()), Oy2, thickness); } } @@ -438,14 +441,15 @@ class vpDetectorAprilTag::Impl const std::vector &corners = tagsCorners[i]; assert(corners.size() == 4); + const unsigned int cornerId0 = 0, cornerId1 = 1, cornerId2 = 2, cornerId3 = 3; - vpDisplay::displayLine(I, static_cast(corners[0].get_i()), static_cast(corners[0].get_j()), static_cast(corners[1].get_i()), static_cast(corners[1].get_j()), + vpDisplay::displayLine(I, static_cast(corners[cornerId0].get_i()), static_cast(corners[cornerId0].get_j()), static_cast(corners[cornerId1].get_i()), static_cast(corners[cornerId1].get_j()), Ox, thickness); - vpDisplay::displayLine(I, static_cast(corners[0].get_i()), static_cast(corners[0].get_j()), static_cast(corners[3].get_i()), static_cast(corners[3].get_j()), + vpDisplay::displayLine(I, static_cast(corners[cornerId0].get_i()), static_cast(corners[cornerId0].get_j()), static_cast(corners[cornerId3].get_i()), static_cast(corners[cornerId3].get_j()), Oy, thickness); - vpDisplay::displayLine(I, static_cast(corners[1].get_i()), static_cast(corners[1].get_j()), static_cast(corners[2].get_i()), static_cast(corners[2].get_j()), + vpDisplay::displayLine(I, static_cast(corners[cornerId1].get_i()), static_cast(corners[cornerId1].get_j()), static_cast(corners[cornerId2].get_i()), static_cast(corners[cornerId2].get_j()), Ox2, thickness); - vpDisplay::displayLine(I, static_cast(corners[2].get_i()), static_cast(corners[2].get_j()), static_cast(corners[3].get_i()), static_cast(corners[3].get_j()), + vpDisplay::displayLine(I, static_cast(corners[cornerId2].get_i()), static_cast(corners[cornerId2].get_j()), static_cast(corners[cornerId3].get_i()), static_cast(corners[cornerId3].get_j()), Oy2, thickness); } } @@ -548,19 +552,21 @@ class vpDetectorAprilTag::Impl pt.set_y(y); pts[1] = pt; + const int idCorner2 = 2; pt.setWorldCoordinates(tagSize / 2.0, -tagSize / 2.0, 0.0); - imPt.set_uv(det->p[2][0], det->p[2][1]); + imPt.set_uv(det->p[idCorner2][0], det->p[idCorner2][1]); vpPixelMeterConversion::convertPoint(cam, imPt, x, y); pt.set_x(x); pt.set_y(y); - pts[2] = pt; + pts[idCorner2] = pt; + const int idCorner3 = 3; pt.setWorldCoordinates(-tagSize / 2.0, -tagSize / 2.0, 0.0); - imPt.set_uv(det->p[3][0], det->p[3][1]); + imPt.set_uv(det->p[idCorner3][0], det->p[idCorner3][1]); vpPixelMeterConversion::convertPoint(cam, imPt, x, y); pt.set_x(x); pt.set_y(y); - pts[3] = pt; + pts[idCorner3] = pt; pose.addPoints(pts); @@ -614,24 +620,27 @@ class vpDetectorAprilTag::Impl double data_p1[] = { scale, scale, 0 }; double data_p2[] = { scale, -scale, 0 }; double data_p3[] = { -scale, -scale, 0 }; - matd_t *p[4] = { matd_create_data(3, 1, data_p0), matd_create_data(3, 1, data_p1), - matd_create_data(3, 1, data_p2), matd_create_data(3, 1, data_p3) }; - matd_t *v[4]; - for (int i = 0; i < 4; ++i) { + const unsigned int nbPoints = 4; + const int nbRows = 3; + matd_t *p[nbPoints] = { matd_create_data(nbRows, 1, data_p0), matd_create_data(nbRows, 1, data_p1), + matd_create_data(nbRows, 1, data_p2), matd_create_data(nbRows, 1, data_p3) }; + matd_t *v[nbPoints]; + for (unsigned int i = 0; i < nbPoints; ++i) { double data_v[] = { (det->p[i][0] - cam.get_u0()) / cam.get_px(), (det->p[i][1] - cam.get_v0()) / cam.get_py(), 1 }; - v[i] = matd_create_data(3, 1, data_v); + v[i] = matd_create_data(nbRows, 1, data_v); } apriltag_pose_t solution1, solution2; const int nIters = 50; - solution1.R = matd_create_data(3, 3, cMo.getRotationMatrix().data); - solution1.t = matd_create_data(3, 1, cMo.getTranslationVector().data); + const int nbCols = 3; + solution1.R = matd_create_data(nbRows, nbCols, cMo.getRotationMatrix().data); + solution1.t = matd_create_data(nbRows, 1, cMo.getTranslationVector().data); double err2; get_second_solution(v, p, &solution1, &solution2, nIters, &err2); - for (int i = 0; i < 4; ++i) { + for (unsigned int i = 0; i < nbPoints; ++i) { matd_destroy(p[i]); matd_destroy(v[i]); } @@ -657,17 +666,18 @@ class vpDetectorAprilTag::Impl } if (!m_zAlignedWithCameraFrame) { + const unsigned int idX = 0, idY = 1, idZ = 2; vpHomogeneousMatrix oMo; // Apply a rotation of 180deg around x axis - oMo[0][0] = 1; - oMo[0][1] = 0; - oMo[0][2] = 0; - oMo[1][0] = 0; - oMo[1][1] = -1; - oMo[1][2] = 0; - oMo[2][0] = 0; - oMo[2][1] = 0; - oMo[2][2] = -1; + oMo[idX][idX] = 1; + oMo[idX][idY] = 0; + oMo[idX][idZ] = 0; + oMo[idY][idX] = 0; + oMo[idY][idY] = -1; + oMo[idY][idZ] = 0; + oMo[idZ][idX] = 0; + oMo[idZ][idY] = 0; + oMo[idZ][idZ] = -1; cMo = cMo * oMo; if (cMo2) { *cMo2 = *cMo2 * oMo; @@ -682,7 +692,8 @@ class vpDetectorAprilTag::Impl { apriltag_pose_t pose1, pose2; double err_1, err_2; - estimate_tag_pose_orthogonal_iteration(&info, &err_1, &pose1, &err_2, &pose2, 50); + const unsigned int nbIters = 50; + estimate_tag_pose_orthogonal_iteration(&info, &err_1, &pose1, &err_2, &pose2, nbIters); if (err_1 <= err_2) { convertHomogeneousMatrix(pose1, cMo1); if (cMo2) { @@ -822,15 +833,19 @@ class vpDetectorAprilTag::Impl }; #endif // DOXYGEN_SHOULD_SKIP_THIS +namespace +{ +const unsigned int def_tagThickness = 2; +} vpDetectorAprilTag::vpDetectorAprilTag(const vpAprilTagFamily &tagFamily, const vpPoseEstimationMethod &poseEstimationMethod) - : m_displayTag(false), m_displayTagColor(vpColor::none), m_displayTagThickness(2), + : m_displayTag(false), m_displayTagColor(vpColor::none), m_displayTagThickness(def_tagThickness), m_poseEstimationMethod(poseEstimationMethod), m_tagFamily(tagFamily), m_defaultCam(), m_impl(new Impl(tagFamily, poseEstimationMethod)) { } vpDetectorAprilTag::vpDetectorAprilTag(const vpDetectorAprilTag &o) - : vpDetectorBase(o), m_displayTag(false), m_displayTagColor(vpColor::none), m_displayTagThickness(2), + : vpDetectorBase(o), m_displayTag(false), m_displayTagColor(vpColor::none), m_displayTagThickness(def_tagThickness), m_poseEstimationMethod(o.m_poseEstimationMethod), m_tagFamily(o.m_tagFamily), m_defaultCam(), m_impl(new Impl(*o.m_impl)) { } @@ -1022,15 +1037,15 @@ std::vector > vpDetectorAprilTag::getTagsPoints3D(const std std::vector > tagsPoints3D; double default_size = -1; - { - std::map::const_iterator it = tagsSize.find(-1); - if (it != tagsSize.end()) { - default_size = it->second; // Default size - } + + std::map::const_iterator it = tagsSize.find(-1); + if (it != tagsSize.end()) { + default_size = it->second; // Default size } + size_t tagsid_size = tagsId.size(); for (size_t i = 0; i < tagsid_size; ++i) { - std::map::const_iterator it = tagsSize.find(tagsId[i]); + it = tagsSize.find(tagsId[i]); double tagSize = default_size; // Default size if (it == tagsSize.end()) { if (default_size < 0) { // no default size found @@ -1042,17 +1057,19 @@ std::vector > vpDetectorAprilTag::getTagsPoints3D(const std tagSize = it->second; } std::vector points3D(4); + const unsigned int idX = 0, idY = 1, idZ = 2, idHomogen = 3; + const double middleFactor = 2.0; if (m_impl->getZAlignedWithCameraAxis()) { - points3D[0] = vpPoint(-tagSize / 2, tagSize / 2, 0); - points3D[1] = vpPoint(tagSize / 2, tagSize / 2, 0); - points3D[2] = vpPoint(tagSize / 2, -tagSize / 2, 0); - points3D[3] = vpPoint(-tagSize / 2, -tagSize / 2, 0); + points3D[idX] = vpPoint(-tagSize / middleFactor, tagSize / middleFactor, 0); + points3D[idY] = vpPoint(tagSize / middleFactor, tagSize / middleFactor, 0); + points3D[idZ] = vpPoint(tagSize / middleFactor, -tagSize / middleFactor, 0); + points3D[idHomogen] = vpPoint(-tagSize / middleFactor, -tagSize / middleFactor, 0); } else { - points3D[0] = vpPoint(-tagSize / 2, -tagSize / 2, 0); - points3D[1] = vpPoint(tagSize / 2, -tagSize / 2, 0); - points3D[2] = vpPoint(tagSize / 2, tagSize / 2, 0); - points3D[3] = vpPoint(-tagSize / 2, tagSize / 2, 0); + points3D[idX] = vpPoint(-tagSize / middleFactor, -tagSize / middleFactor, 0); + points3D[idY] = vpPoint(tagSize / middleFactor, -tagSize / middleFactor, 0); + points3D[idZ] = vpPoint(tagSize / middleFactor, tagSize / middleFactor, 0); + points3D[idHomogen] = vpPoint(-tagSize / middleFactor, tagSize / middleFactor, 0); } tagsPoints3D.push_back(points3D); } @@ -1197,16 +1214,7 @@ void vpDetectorAprilTag::setZAlignedWithCameraAxis(bool zAlignedWithCameraFrame) { m_impl->setZAlignedWithCameraAxis(zAlignedWithCameraFrame); } - END_VISP_NAMESPACE - -VISP_EXPORT void swap(VISP_NAMESPACE_ADDRESSING vpDetectorAprilTag &o1, VISP_NAMESPACE_ADDRESSING vpDetectorAprilTag &o2) -{ - using std::swap; - - swap(o1.m_impl, o2.m_impl); -} - #elif !defined(VISP_BUILD_SHARED_LIBS) // Work around to avoid warning: libvisp_core.a(vpDetectorAprilTag.cpp.o) has // no symbols diff --git a/modules/detection/src/vpDetectorBase.cpp b/modules/detection/src/vpDetectorBase.cpp index 03801c7abf..317304be13 100644 --- a/modules/detection/src/vpDetectorBase.cpp +++ b/modules/detection/src/vpDetectorBase.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/gui/include/visp3/gui/vpD3DRenderer.h b/modules/gui/include/visp3/gui/vpD3DRenderer.h index b1b6b5d6a0..586ca8c3a2 100644 --- a/modules/gui/include/visp3/gui/vpD3DRenderer.h +++ b/modules/gui/include/visp3/gui/vpD3DRenderer.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,8 +31,8 @@ * D3D renderer for windows 32 display */ -#ifndef VPD3DRENDERER_HH -#define VPD3DRENDERER_HH +#ifndef VP_D3D_RENDERER_H +#define VP_D3D_RENDERER_H #include diff --git a/modules/gui/include/visp3/gui/vpDisplayD3D.h b/modules/gui/include/visp3/gui/vpDisplayD3D.h index 5e3b8bb001..75def030fe 100644 --- a/modules/gui/include/visp3/gui/vpDisplayD3D.h +++ b/modules/gui/include/visp3/gui/vpDisplayD3D.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,8 +31,8 @@ * Windows 32 display using D3D */ -#ifndef VPDISPLAYD3D_HH -#define VPDISPLAYD3D_HH +#ifndef VP_DISPLAY_D3D_H +#define VP_DISPLAY_D3D_H #include #include diff --git a/modules/gui/include/visp3/gui/vpDisplayGDI.h b/modules/gui/include/visp3/gui/vpDisplayGDI.h index cae289c385..c0a86550ab 100644 --- a/modules/gui/include/visp3/gui/vpDisplayGDI.h +++ b/modules/gui/include/visp3/gui/vpDisplayGDI.h @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,15 +29,10 @@ * * Description: * Windows 32 display using GDI - * - * Authors: - * Bruno Renier - * -*****************************************************************************/ - + */ -#ifndef vpDisplayGDI_HH -#define vpDisplayGDI_HH +#ifndef VP_DISPLAY_GDI_H +#define VP_DISPLAY_GDI_H #include #include @@ -128,7 +122,7 @@ BEGIN_VISP_NAMESPACE * } * \endcode */ -class VISP_EXPORT vpDisplayGDI : public vpDisplayWin32 + class VISP_EXPORT vpDisplayGDI : public vpDisplayWin32 { public: vpDisplayGDI(); diff --git a/modules/gui/include/visp3/gui/vpDisplayGTK.h b/modules/gui/include/visp3/gui/vpDisplayGTK.h index de6ae7e297..61a3ba1ee6 100644 --- a/modules/gui/include/visp3/gui/vpDisplayGTK.h +++ b/modules/gui/include/visp3/gui/vpDisplayGTK.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,8 +31,8 @@ * Image display. */ -#ifndef vpDisplayGTK_h -#define vpDisplayGTK_h +#ifndef VP_DISPLAY_GTK_H +#define VP_DISPLAY_GTK_H #include #include diff --git a/modules/gui/include/visp3/gui/vpDisplayOpenCV.h b/modules/gui/include/visp3/gui/vpDisplayOpenCV.h index b45b81c416..ff4e84fafb 100644 --- a/modules/gui/include/visp3/gui/vpDisplayOpenCV.h +++ b/modules/gui/include/visp3/gui/vpDisplayOpenCV.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,8 +31,8 @@ * Image display. */ -#ifndef _vpDisplayOpenCV_h_ -#define _vpDisplayOpenCV_h_ +#ifndef VP_DISPLAY_OPENCV_H +#define VP_DISPLAY_OPENCV_H #include #include diff --git a/modules/gui/include/visp3/gui/vpDisplayWin32.h b/modules/gui/include/visp3/gui/vpDisplayWin32.h index 490b24db82..9f6734ec50 100644 --- a/modules/gui/include/visp3/gui/vpDisplayWin32.h +++ b/modules/gui/include/visp3/gui/vpDisplayWin32.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -32,8 +32,8 @@ */ -#ifndef vpDisplayWin32_hh -#define vpDisplayWin32_hh +#ifndef VP_DISPLAY_WIN32_H +#define VP_DISPLAY_WIN32_H #include #include diff --git a/modules/gui/include/visp3/gui/vpDisplayX.h b/modules/gui/include/visp3/gui/vpDisplayX.h index d94e9b728e..5ad842eb1f 100644 --- a/modules/gui/include/visp3/gui/vpDisplayX.h +++ b/modules/gui/include/visp3/gui/vpDisplayX.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,8 +36,8 @@ * \brief Define the X11 console to display images. */ -#ifndef vpDisplayX_h -#define vpDisplayX_h +#ifndef VP_DISPLAY_X_H +#define VP_DISPLAY_X_H #include #include diff --git a/modules/gui/include/visp3/gui/vpGDIRenderer.h b/modules/gui/include/visp3/gui/vpGDIRenderer.h index ad372d31e9..894b2a9aa4 100644 --- a/modules/gui/include/visp3/gui/vpGDIRenderer.h +++ b/modules/gui/include/visp3/gui/vpGDIRenderer.h @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,15 +29,10 @@ * * Description: * GDI renderer for windows 32 display - * - * Authors: - * Bruno Renier - * -*****************************************************************************/ - + */ -#ifndef vpGDIRenderer_HH -#define vpGDIRenderer_HH +#ifndef VP_GDI_RENDERER_H +#define VP_GDI_RENDERER_H #include diff --git a/modules/gui/include/visp3/gui/vpPlot.h b/modules/gui/include/visp3/gui/vpPlot.h index 9a4661eb48..f028c4fbf9 100644 --- a/modules/gui/include/visp3/gui/vpPlot.h +++ b/modules/gui/include/visp3/gui/vpPlot.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,8 +36,8 @@ \brief Plot curves. */ -#ifndef vpPlot_H -#define vpPlot_H +#ifndef VP_PLOT_H +#define VP_PLOT_H #include #include diff --git a/modules/gui/include/visp3/gui/vpPlotCurve.h b/modules/gui/include/visp3/gui/vpPlotCurve.h index f9ce399718..c616da76ad 100644 --- a/modules/gui/include/visp3/gui/vpPlotCurve.h +++ b/modules/gui/include/visp3/gui/vpPlotCurve.h @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,13 +29,14 @@ * * Description: * Define a curve for the vpPlot class. - * -*****************************************************************************/ + */ -#ifndef DOXYGEN_SHOULD_SKIP_THIS +#ifndef VP_PLOT_CURVE_H +#define VP_PLOT_CURVE_H -#ifndef vpPlotCurve_H -#define vpPlotCurve_H +#include + +#ifndef DOXYGEN_SHOULD_SKIP_THIS #include #include @@ -61,9 +61,6 @@ class vpPlotCurve vpColor color; vpCurveStyle curveStyle; unsigned int thickness; - // vpMarkerStyle markerStyle; - // char lineStyle[20]; - // vpList pointList; unsigned int nbPoint; vpImagePoint lastPoint; std::list pointListx; diff --git a/modules/gui/include/visp3/gui/vpPlotGraph.h b/modules/gui/include/visp3/gui/vpPlotGraph.h index c2605fa4b3..084deb1e00 100644 --- a/modules/gui/include/visp3/gui/vpPlotGraph.h +++ b/modules/gui/include/visp3/gui/vpPlotGraph.h @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,10 @@ * * Description: * Define a graph for the vpPlot class. - * -*****************************************************************************/ + */ -#ifndef vpPlotGraph_H -#define vpPlotGraph_H +#ifndef VP_PLOT_GRAPH_H +#define VP_PLOT_GRAPH_H #include diff --git a/modules/gui/include/visp3/gui/vpProjectionDisplay.h b/modules/gui/include/visp3/gui/vpProjectionDisplay.h index 84240bf39b..a05bfe5b01 100644 --- a/modules/gui/include/visp3/gui/vpProjectionDisplay.h +++ b/modules/gui/include/visp3/gui/vpProjectionDisplay.h @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,10 @@ * * Description: * Interface with the image for feature display. - * -*****************************************************************************/ + */ -#ifndef vpProjectionDisplay_H -#define vpProjectionDisplay_H +#ifndef VP_PROJECTION_DISPLAY_H +#define VP_PROJECTION_DISPLAY_H /*! \file vpProjectionDisplay.h diff --git a/modules/gui/include/visp3/gui/vpWin32API.h b/modules/gui/include/visp3/gui/vpWin32API.h index 060a56bac0..9cbc1697ba 100644 --- a/modules/gui/include/visp3/gui/vpWin32API.h +++ b/modules/gui/include/visp3/gui/vpWin32API.h @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,14 +29,10 @@ * * Description: * GDI renderer for windows 32 display - * - * Authors: - * Filip Novotny - * -*****************************************************************************/ + */ -#ifndef vpWin32API_HH -#define vpWin32API_HH +#ifndef VP_WIN32_API_H +#define VP_WIN32_API_H #include diff --git a/modules/gui/include/visp3/gui/vpWin32Renderer.h b/modules/gui/include/visp3/gui/vpWin32Renderer.h index 70f6cac00b..313e90b39e 100644 --- a/modules/gui/include/visp3/gui/vpWin32Renderer.h +++ b/modules/gui/include/visp3/gui/vpWin32Renderer.h @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,15 +29,10 @@ * * Description: * Windows 32 renderer base class - * - * Authors: - * Bruno Renier - * -*****************************************************************************/ - + */ -#ifndef vpWin32Renderer_HH -#define vpWin32Renderer_HH +#ifndef VP_WIN32_RENDERER_H +#define VP_WIN32_RENDERER_H #include @@ -51,7 +45,6 @@ // Include WinSock2.h before windows.h to ensure that winsock.h is not // included by windows.h since winsock.h and winsock2.h are incompatible #include -#include #include BEGIN_VISP_NAMESPACE diff --git a/modules/gui/include/visp3/gui/vpWin32Window.h b/modules/gui/include/visp3/gui/vpWin32Window.h index 8cc48ad774..b96f73c0b3 100644 --- a/modules/gui/include/visp3/gui/vpWin32Window.h +++ b/modules/gui/include/visp3/gui/vpWin32Window.h @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,15 +29,10 @@ * * Description: * Windows 32 display's window class - * - * Authors: - * Bruno Renier - * Anthony Saunier - * -*****************************************************************************/ + */ -#ifndef vpWin32Window_HH -#define vpWin32Window_HH +#ifndef VP_WIN32_WINDOW_H +#define VP_WIN32_WINDOW_H #include diff --git a/modules/gui/src/display/vpDisplayGTK.cpp b/modules/gui/src/display/vpDisplayGTK.cpp index 677f2c260f..7a4acc58ed 100644 --- a/modules/gui/src/display/vpDisplayGTK.cpp +++ b/modules/gui/src/display/vpDisplayGTK.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,7 @@ * * Description: * Image display. - * - * Authors: - * Christophe Collewet - * -*****************************************************************************/ + */ /*! \file vpDisplayGTK.cpp @@ -56,7 +51,6 @@ #include // debug / exception -#include #include #include #include @@ -985,7 +979,10 @@ void vpDisplayGTK::displayImageROI(const vpImage &I, const vpImagePoint \sa init(), closeDisplay() */ -void vpDisplayGTK::displayImage(const unsigned char * /* I */) { vpTRACE(" not implemented "); } +void vpDisplayGTK::displayImage(const unsigned char * /* I */) +{ + // not implemented +} /*! Close the window. @@ -1033,7 +1030,10 @@ void vpDisplayGTK::flushDisplayROI(const vpImagePoint & /*iP*/, const unsigned i /*! \warning Not implemented yet. */ -void vpDisplayGTK::clearDisplay(const vpColor & /* color */) { vpTRACE("Not implemented"); } +void vpDisplayGTK::clearDisplay(const vpColor & /* color */) +{ + // Not implemented +} /*! Display an arrow from image point \e ip1 to image point \e ip2. diff --git a/modules/gui/src/display/vpDisplayOpenCV.cpp b/modules/gui/src/display/vpDisplayOpenCV.cpp index e6e850bc47..3fa964631e 100644 --- a/modules/gui/src/display/vpDisplayOpenCV.cpp +++ b/modules/gui/src/display/vpDisplayOpenCV.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -56,7 +55,6 @@ #include // debug / exception -#include #include #include // for CV_FILLED versus cv::FILLED @@ -477,7 +475,10 @@ void vpDisplayOpenCV::init(unsigned int w, unsigned int h, int x, int y, const s \sa displayText() */ -void vpDisplayOpenCV::setFont(const std::string & /* font */) { vpERROR_TRACE("Not yet implemented"); } +void vpDisplayOpenCV::setFont(const std::string & /* font */) +{ + // Not yet implemented +} /*! Set the window title. @@ -488,11 +489,7 @@ void vpDisplayOpenCV::setFont(const std::string & /* font */) { vpERROR_TRACE("N */ void vpDisplayOpenCV::setTitle(const std::string & /* title */) { - // static bool warn_displayed = false; - // if (! warn_displayed) { - // vpTRACE("Not implemented"); - // warn_displayed = true; - // } + // Not implemented } /*! @@ -746,7 +743,10 @@ void vpDisplayOpenCV::displayImageROI(const vpImage &I, const vpImagePoi \sa init(), closeDisplay() */ -void vpDisplayOpenCV::displayImage(const unsigned char * /* I */) { vpTRACE(" not implemented "); } +void vpDisplayOpenCV::displayImage(const unsigned char * /* I */) +{ + // not implemented +} /*! @@ -814,11 +814,7 @@ void vpDisplayOpenCV::flushDisplayROI(const vpImagePoint & /*iP*/, const unsigne */ void vpDisplayOpenCV::clearDisplay(const vpColor & /* color */) { - static bool warn_displayed = false; - if (!warn_displayed) { - vpTRACE("Not implemented"); - warn_displayed = true; - } + // Not implemented } /*! diff --git a/modules/gui/src/display/vpDisplayX.cpp b/modules/gui/src/display/vpDisplayX.cpp index ee979b50be..371fcddc58 100644 --- a/modules/gui/src/display/vpDisplayX.cpp +++ b/modules/gui/src/display/vpDisplayX.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,7 @@ * * Description: * Image display. - * - * Authors: - * Anthony Saunier - * -*****************************************************************************/ + */ /*! \file vpDisplayX.cpp @@ -55,7 +50,6 @@ #include // debug / exception -#include #include // math @@ -203,11 +197,13 @@ class vpDisplayX::Impl while (i < size) { unsigned char nivGris = src_8[i]; - if (nivGris > nivGrisMax) + if (nivGris > nivGrisMax) { dst_8[i] = 255; - else + } + else { dst_8[i] = nivGris; - i++; + } + ++i; } } else { @@ -476,11 +472,11 @@ class vpDisplayX::Impl *(dst_8 + j) = 255; else *(dst_8 + j) = nivGris; - j++; + ++j; } src_8 = src_8 + iwidth; dst_8 = dst_8 + width; - i++; + ++i; } XPutImage(display, pixmap, context, Ximage, (int)iP.get_u(), (int)iP.get_v(), (int)iP.get_u(), (int)iP.get_v(), @@ -574,11 +570,11 @@ class vpDisplayX::Impl *(dst_32 + 4 * j + 1) = val; *(dst_32 + 4 * j + 2) = val; *(dst_32 + 4 * j + 3) = val; - j++; + ++j; } src_8 = src_8 + iwidth; dst_32 = dst_32 + 4 * width; - i++; + ++i; } } else { @@ -592,11 +588,11 @@ class vpDisplayX::Impl *(dst_32 + 4 * j + 1) = val; *(dst_32 + 4 * j + 2) = val; *(dst_32 + 4 * j + 3) = vpRGBa::alpha_default; - j++; + ++j; } src_8 = src_8 + iwidth; dst_32 = dst_32 + 4 * width; - i++; + ++i; } } @@ -730,11 +726,11 @@ class vpDisplayX::Impl *(dst_32 + 4 * j + 2) = (src_32 + j)->G; *(dst_32 + 4 * j + 3) = (src_32 + j)->B; - j++; + ++j; } src_32 = src_32 + iwidth; dst_32 = dst_32 + 4 * width; - i++; + ++i; } } @@ -748,11 +744,11 @@ class vpDisplayX::Impl *(dst_32 + 4 * j + 2) = (src_32 + j)->R; *(dst_32 + 4 * j + 3) = (src_32 + j)->A; - j++; + ++j; } src_32 = src_32 + iwidth; dst_32 = dst_32 + 4 * width; - i++; + ++i; } } @@ -1177,19 +1173,15 @@ class vpDisplayX::Impl } if ((display = XOpenDisplay(nullptr)) == nullptr) { - vpERROR_TRACE("Can't connect display on server %s.\n", XDisplayName(nullptr)); - throw(vpDisplayException(vpDisplayException::connexionError, "Can't connect display on server.")); + throw(vpDisplayException(vpDisplayException::connexionError, "Can't connect display on server %s.", XDisplayName(nullptr))); } screen = DefaultScreen(display); lut = DefaultColormap(display, screen); screen_depth = (unsigned int)DefaultDepth(display, screen); - vpTRACE("Screen depth: %d\n", screen_depth); - if ((window = XCreateSimpleWindow(display, RootWindow(display, screen), win_x, win_y, win_width, win_height, 1, BlackPixel(display, screen), WhitePixel(display, screen))) == 0) { - vpERROR_TRACE("Can't create window."); throw(vpDisplayException(vpDisplayException::cannotOpenWindowError, "Can't create window.")); } @@ -1217,8 +1209,7 @@ class vpDisplayX::Impl xcolor.pad = 0; xcolor.red = xcolor.green = xcolor.blue = 256 * i; if (XAllocColor(display, lut, &xcolor) == 0) { - vpERROR_TRACE("Can't allocate 256 colors. Only %d allocated.", i); - throw(vpDisplayException(vpDisplayException::colorAllocError, "Can't allocate 256 colors.")); + throw(vpDisplayException(vpDisplayException::colorAllocError, "Can't allocate 256 colors. Only %d allocated.", i)); } colortable[i] = xcolor.pixel; } @@ -1563,7 +1554,6 @@ class vpDisplayX::Impl context = XCreateGC(display, window, GCPlaneMask | GCFillStyle | GCForeground | GCBackground, &values); if (context == nullptr) { - vpERROR_TRACE("Can't create graphics context."); throw(vpDisplayException(vpDisplayException::XWindowsError, "Can't create graphics context")); } diff --git a/modules/gui/src/display/windows/vpD3DRenderer.cpp b/modules/gui/src/display/windows/vpD3DRenderer.cpp index e8a1fc1134..31c75817c6 100644 --- a/modules/gui/src/display/windows/vpD3DRenderer.cpp +++ b/modules/gui/src/display/windows/vpD3DRenderer.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,7 @@ * * Description: * D3D renderer for windows 32 display - * - * Authors: - * Bruno Renier - * -*****************************************************************************/ + */ #include @@ -461,7 +456,7 @@ void vpD3DRenderer::setImg(const vpImage &im) // locks the texture to directly access it if (pd3dText->LockRect(0, &d3dLRect, &r, 0) != D3D_OK) { - vpCERROR << "D3D : Couldn't lock the texture!" << std::endl; + std::cout << "D3D : Couldn't lock the texture!" << std::endl; return; } @@ -474,7 +469,7 @@ void vpD3DRenderer::setImg(const vpImage &im) // unlocks the texture if (pd3dText->UnlockRect(0) != D3D_OK) - vpCERROR << "D3D : Couldn't unlock the texture!" << std::endl; + std::cout << "D3D : Couldn't unlock the texture!" << std::endl; } } @@ -502,7 +497,7 @@ void vpD3DRenderer::setImgROI(const vpImage &im, const vpImagePoint &iP, // locks the texture to directly access it if (pd3dText->LockRect(0, &d3dLRect, &r, 0) != D3D_OK) { - vpCERROR << "D3D : Couldn't lock the texture!" << std::endl; + std::cout << "D3D : Couldn't lock the texture!" << std::endl; return; } @@ -515,7 +510,7 @@ void vpD3DRenderer::setImgROI(const vpImage &im, const vpImagePoint &iP, // unlocks the texture if (pd3dText->UnlockRect(0) != D3D_OK) - vpCERROR << "D3D : Couldn't unlock the texture!" << std::endl; + std::cout << "D3D : Couldn't unlock the texture!" << std::endl; } } @@ -537,7 +532,7 @@ void vpD3DRenderer::setImg(const vpImage &im) // locks the texture to directly access it if (pd3dText->LockRect(0, &d3dLRect, &r, 0) != D3D_OK) { - vpCERROR << "D3D : Couldn't lock the texture!" << std::endl; + std::cout << "D3D : Couldn't lock the texture!" << std::endl; return; } @@ -550,7 +545,7 @@ void vpD3DRenderer::setImg(const vpImage &im) // unlocks the texture if (pd3dText->UnlockRect(0) != D3D_OK) - vpCERROR << "D3D : Couldn't unlock the texture!" << std::endl; + std::cout << "D3D : Couldn't unlock the texture!" << std::endl; } } @@ -578,7 +573,7 @@ void vpD3DRenderer::setImgROI(const vpImage &im, const vpImagePoi // locks the texture to directly access it if (pd3dText->LockRect(0, &d3dLRect, &r, 0) != D3D_OK) { - vpCERROR << "D3D : Couldn't lock the texture!" << std::endl; + std::cout << "D3D : Couldn't lock the texture!" << std::endl; return; } @@ -591,7 +586,7 @@ void vpD3DRenderer::setImgROI(const vpImage &im, const vpImagePoi // unlocks the texture if (pd3dText->UnlockRect(0) != D3D_OK) - vpCERROR << "D3D : Couldn't unlock the texture!" << std::endl; + std::cout << "D3D : Couldn't unlock the texture!" << std::endl; } } @@ -666,7 +661,7 @@ void vpD3DRenderer::setPixel(const vpImagePoint &iP, const vpColor &color) // locks the texture to directly access it if (pd3dText->LockRect(0, &d3dLRect, &r, 0) != D3D_OK) { - vpCERROR << "D3D : Couldn't lock the texture!" << std::endl; + std::cout << "D3D : Couldn't lock the texture!" << std::endl; return; } @@ -679,7 +674,7 @@ void vpD3DRenderer::setPixel(const vpImagePoint &iP, const vpColor &color) // unlocks the texture if (pd3dText->UnlockRect(0) != D3D_OK) - vpCERROR << "D3D : Couldn't unlock the texture!" << std::endl; + std::cout << "D3D : Couldn't unlock the texture!" << std::endl; } } @@ -808,7 +803,6 @@ void vpD3DRenderer::drawRect(const vpImagePoint &topLeft, unsigned int width, un if (topLeftScaled.get_i() > (int)m_rheight - 1 || topLeftScaled.get_j() > (int)m_rwidth - 1 || topLeftScaled.get_i() + height < 0 || topLeftScaled.get_j() + width < 0) { - // vpCERROR<<"Invalid parameters!"<LockRect(0, &d3dLRect, &r, 0) != D3D_OK) { - vpCERROR << "D3D : Couldn't lock the texture!" << std::endl; + std::cout << "D3D : Couldn't lock the texture!" << std::endl; return; } @@ -845,7 +839,7 @@ void vpD3DRenderer::drawRect(const vpImagePoint &topLeft, unsigned int width, un // unlocks the texture if (pd3dText->UnlockRect(0) != D3D_OK) - vpCERROR << "D3D : Couldn't unlock the texture!" << std::endl; + std::cout << "D3D : Couldn't unlock the texture!" << std::endl; } } } @@ -868,7 +862,7 @@ void vpD3DRenderer::clear(const vpColor &color) // locks the texture to directly access it if (pd3dText->LockRect(0, &d3dLRect, &r, 0) != D3D_OK) { - vpCERROR << "D3D : Couldn't lock the texture!" << std::endl; + std::cout << "D3D : Couldn't lock the texture!" << std::endl; return; } @@ -890,7 +884,7 @@ void vpD3DRenderer::clear(const vpColor &color) // unlocks the texture if (pd3dText->UnlockRect(0) != D3D_OK) - vpCERROR << "D3D : Couldn't unlock the texture!" << std::endl; + std::cout << "D3D : Couldn't unlock the texture!" << std::endl; } } @@ -968,7 +962,7 @@ void vpD3DRenderer::drawCircle(const vpImagePoint ¢er, unsigned int radius, // locks the texture to directly access it if (pd3dText->LockRect(0, &d3dLRect, &rec, 0) != D3D_OK) { - vpCERROR << "D3D : Couldn't lock the texture!" << std::endl; + std::cout << "D3D : Couldn't lock the texture!" << std::endl; return; } @@ -1001,7 +995,7 @@ void vpD3DRenderer::drawCircle(const vpImagePoint ¢er, unsigned int radius, // unlocks the texture if (pd3dText->UnlockRect(0) != D3D_OK) - vpCERROR << "D3D : Couldn't unlock the texture!" << std::endl; + std::cout << "D3D : Couldn't unlock the texture!" << std::endl; } } @@ -1176,7 +1170,7 @@ void vpD3DRenderer::getImage(vpImage &I) // locks the whole texture to directly access it if (pd3dText->LockRect(0, &d3dLRect, &r, 0) != D3D_OK) { - vpCERROR << "D3D : Couldn't lock the texture!" << std::endl; + std::cout << "D3D : Couldn't lock the texture!" << std::endl; return; } @@ -1189,7 +1183,7 @@ void vpD3DRenderer::getImage(vpImage &I) // unlocks the texture if (pd3dText->UnlockRect(0) != D3D_OK) - vpCERROR << "D3D : Couldn't unlock the texture!" << std::endl; + std::cout << "D3D : Couldn't unlock the texture!" << std::endl; } } diff --git a/modules/gui/src/display/windows/vpDisplayD3D.cpp b/modules/gui/src/display/windows/vpDisplayD3D.cpp index 6932f1b755..5c9ed60af3 100644 --- a/modules/gui/src/display/windows/vpDisplayD3D.cpp +++ b/modules/gui/src/display/windows/vpDisplayD3D.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,7 @@ * * Description: * windows 32 display using D3D - * - * Authors: - * Bruno Renier - * -*****************************************************************************/ + */ /*! \file vpDisplayD3D.cpp diff --git a/modules/gui/src/display/windows/vpDisplayGDI.cpp b/modules/gui/src/display/windows/vpDisplayGDI.cpp index 0110450efc..09ac565b93 100644 --- a/modules/gui/src/display/windows/vpDisplayGDI.cpp +++ b/modules/gui/src/display/windows/vpDisplayGDI.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,7 @@ * * Description: * GDI based Display for windows 32. - * - * Authors: - * Bruno Renier - * -*****************************************************************************/ + */ /*! \file vpDisplayGDI.cpp \brief GDI based Display for windows 32. diff --git a/modules/gui/src/display/windows/vpDisplayWin32.cpp b/modules/gui/src/display/windows/vpDisplayWin32.cpp index 16616395fb..33b75e99d0 100644 --- a/modules/gui/src/display/windows/vpDisplayWin32.cpp +++ b/modules/gui/src/display/windows/vpDisplayWin32.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,7 @@ * * Description: * Windows 32 display base class - * - * Authors: - * Bruno Renier - * -*****************************************************************************/ + */ #include #if (defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9)) @@ -95,7 +90,6 @@ vpDisplayWin32::~vpDisplayWin32() { closeDisplay(); } void vpDisplayWin32::init(vpImage &I, int x, int y, const std::string &title) { if ((I.getHeight() == 0) || (I.getWidth() == 0)) { - vpERROR_TRACE("Image not initialized "); throw(vpDisplayException(vpDisplayException::notInitializedError, "Image not initialized")); } @@ -119,7 +113,6 @@ void vpDisplayWin32::init(vpImage &I, int x, int y, const std::st void vpDisplayWin32::init(vpImage &I, int x, int y, const std::string &title) { if ((I.getHeight() == 0) || (I.getWidth() == 0)) { - vpERROR_TRACE("Image not initialized "); throw(vpDisplayException(vpDisplayException::notInitializedError, "Image not initialized")); } @@ -631,7 +624,10 @@ void vpDisplayWin32::setTitle(const std::string &windowtitle) \param fontname : Name of the font. */ -void vpDisplayWin32::setFont(const std::string & /* fontname */) { vpERROR_TRACE("Not yet implemented"); } +void vpDisplayWin32::setFont(const std::string & /* fontname */) +{ + // Not yet implemented +} /*! \brief flush the Win32 buffer diff --git a/modules/gui/src/display/windows/vpGDIRenderer.cpp b/modules/gui/src/display/windows/vpGDIRenderer.cpp index 3610f4e284..8bcc1debbc 100644 --- a/modules/gui/src/display/windows/vpGDIRenderer.cpp +++ b/modules/gui/src/display/windows/vpGDIRenderer.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,7 @@ * * Description: * GDI renderer for windows 32 display - * - * Authors: - * Bruno Renier - * -*****************************************************************************/ + */ #include #define GDI_ROBUST @@ -525,6 +520,7 @@ void vpGDIRenderer::drawLine(const vpImagePoint &ip1, const vpImagePoint &ip2, c double start = vpTime::measureTimeMs(); while (vpTime::measureTimeMs() - start < 1000) { hDCScreen = GetDC(m_hWnd); + if (!hDCScreen) continue; hDCMem = CreateCompatibleDC(hDCScreen); diff --git a/modules/gui/src/display/windows/vpWin32API.cpp b/modules/gui/src/display/windows/vpWin32API.cpp index 5e3cdf8adf..beee436ec5 100644 --- a/modules/gui/src/display/windows/vpWin32API.cpp +++ b/modules/gui/src/display/windows/vpWin32API.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,7 @@ * * Description: * GDI renderer for windows 32 display - * - * Authors: - * Filip Novotny - * -*****************************************************************************/ + */ #include #include diff --git a/modules/gui/src/display/windows/vpWin32Window.cpp b/modules/gui/src/display/windows/vpWin32Window.cpp index 34aca6be34..e26eba64e9 100644 --- a/modules/gui/src/display/windows/vpWin32Window.cpp +++ b/modules/gui/src/display/windows/vpWin32Window.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,12 +29,8 @@ * * Description: * Windows 32 display's window class - * - * Authors: - * Bruno Renier - * Anthony Saunier - * -*****************************************************************************/ + */ + #include #include #include diff --git a/modules/gui/src/forward-projection/vpProjectionDisplay.cpp b/modules/gui/src/forward-projection/vpProjectionDisplay.cpp index 266c3895bf..58d0ebee34 100644 --- a/modules/gui/src/forward-projection/vpProjectionDisplay.cpp +++ b/modules/gui/src/forward-projection/vpProjectionDisplay.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/gui/src/plot/vpPlot.cpp b/modules/gui/src/plot/vpPlot.cpp index cf6cd58779..428f0770d1 100644 --- a/modules/gui/src/plot/vpPlot.cpp +++ b/modules/gui/src/plot/vpPlot.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -290,8 +289,9 @@ void vpPlot::plot(unsigned int graphNum, double x, const vpColVector &v_y) for (unsigned int i = 0; i < v_y.getRows(); ++i) this->plot(graphNum, i, x, v_y[i]); } - else - vpTRACE("error in plot vector : not the right dimension"); + else { + throw(vpException(vpException::dimensionError, "Error in plot vector: not the right dimension")); + } } /*! This function enables you to add new points in all curves of a plot. These @@ -311,8 +311,9 @@ void vpPlot::plot(unsigned int graphNum, double x, const vpRowVector &v_y) for (unsigned int i = 0; i < v_y.getRows(); ++i) this->plot(graphNum, i, x, v_y[i]); } - else - vpTRACE("error in plot vector : not the right dimension"); + else { + throw(vpException(vpException::dimensionError, "Error in plot vector: not the right dimension")); + } } /*! @@ -334,8 +335,9 @@ void vpPlot::plot(unsigned int graphNum, double x, const vpPoseVector &v_y) for (unsigned int i = 0; i < v_y.getRows(); ++i) this->plot(graphNum, i, x, v_y[i]); } - else - vpTRACE("error in plot vector : not the right dimension"); + else { + throw(vpException(vpException::dimensionError, "Error in plot vector: not the right dimension")); + } } /*! This function enables you to add new points in all curves of a plot. These @@ -356,8 +358,9 @@ void vpPlot::plot(unsigned int graphNum, double x, const vpTranslationVector &v_ for (unsigned int i = 0; i < v_y.getRows(); ++i) this->plot(graphNum, i, x, v_y[i]); } - else - vpTRACE("error in plot vector : not the right dimension"); + else { + throw(vpException(vpException::dimensionError, "Error in plot vector: not the right dimension")); + } } /*! @@ -379,8 +382,9 @@ void vpPlot::plot(unsigned int graphNum, double x, const vpRotationVector &v_y) for (unsigned int i = 0; i < v_y.size(); ++i) this->plot(graphNum, i, x, v_y[i]); } - else - vpTRACE("error in plot vector : not the right dimension"); + else { + throw(vpException(vpException::dimensionError, "Error in plot vector: not the right dimension")); + } } /*! @@ -429,8 +433,9 @@ vpMouseButton::vpMouseButtonType vpPlot::plot(unsigned int graphNum, double x, c for (unsigned int i = 0; i < v_y.getRows(); ++i) button = this->plot(graphNum, i, x, v_y[i], v_z[i]); } - else - vpTRACE("error in plot vector : not the right dimension"); + else { + throw(vpException(vpException::dimensionError, "Error in plot vector: not the right dimension")); + } return button; } diff --git a/modules/gui/src/plot/vpPlotCurve.cpp b/modules/gui/src/plot/vpPlotCurve.cpp index 0690f9eb33..38ce375e08 100644 --- a/modules/gui/src/plot/vpPlotCurve.cpp +++ b/modules/gui/src/plot/vpPlotCurve.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/gui/src/plot/vpPlotGraph.cpp b/modules/gui/src/plot/vpPlotGraph.cpp index 607ad5250e..ef3aa7c5eb 100644 --- a/modules/gui/src/plot/vpPlotGraph.cpp +++ b/modules/gui/src/plot/vpPlotGraph.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -175,7 +174,8 @@ void vpPlotGraph::findPose() // Instead of pose computation we use an approximation double Z = 0; - for (unsigned int i = 0; i < 4; ++i) { + const unsigned int val_4 = 4; + for (unsigned int i = 0; i < val_4; ++i) { vpPixelMeterConversion::convertPoint(cam, iP[i], x, y); Z = vpMath::maximum(Z, point_[i].get_oX() / x); Z = vpMath::maximum(Z, point_[i].get_oY() / y); @@ -235,18 +235,18 @@ int laFonctionSansNom(double delta) if (d < 1) { while (d < 1) { d = d * 10; - power++; + ++power; } - power--; + --power; return power; } if (d >= 10) { while (d > 10) { d = d / 10; - power--; + --power; } - power--; + --power; return power; } @@ -857,13 +857,14 @@ void getGrid3DPoint(double pente, vpImagePoint &iPunit, vpImagePoint &ip1, vpIma void vpPlotGraph::displayGrid3D(vpImage &I) { + const unsigned int nparam = 6; computeGraphParameters3D(); xdelt = (xmax - xmin) / nbDivisionx; ydelt = (ymax - ymin) / nbDivisiony; zdelt = (zmax - zmin) / nbDivisionz; - vpPoint pt[6]; + vpPoint pt[nparam]; pt[0].setWorldCoordinates(-w_xval, ptYorg, ptZorg); pt[1].setWorldCoordinates(w_xval, ptYorg, ptZorg); pt[2].setWorldCoordinates(ptXorg, -w_yval, ptZorg); @@ -871,8 +872,8 @@ void vpPlotGraph::displayGrid3D(vpImage &I) pt[4].setWorldCoordinates(ptXorg, ptYorg, -w_zval); pt[5].setWorldCoordinates(ptXorg, ptYorg, w_zval); - vpImagePoint iP[6]; - for (unsigned int i = 0; i < 6; ++i) { + vpImagePoint iP[nparam]; + for (unsigned int i = 0; i < nparam; ++i) { pt[i].track(cMo); double u = 0.0, v = 0.0; vpMeterPixelConversion::convertPoint(cam, pt[i].get_x(), pt[i].get_y(), u, v); diff --git a/modules/imgproc/include/visp3/imgproc/vpCircleHoughTransform.h b/modules/imgproc/include/visp3/imgproc/vpCircleHoughTransform.h index 171d5ec6fd..aff4746727 100644 --- a/modules/imgproc/include/visp3/imgproc/vpCircleHoughTransform.h +++ b/modules/imgproc/include/visp3/imgproc/vpCircleHoughTransform.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -28,8 +28,8 @@ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. */ -#ifndef _vpCircleHoughTransform_h_ -#define _vpCircleHoughTransform_h_ +#ifndef VP_CIRCLE_HOUGH_TRANSFORM_H +#define VP_CIRCLE_HOUGH_TRANSFORM_H // System includes #include @@ -67,17 +67,15 @@ class VISP_EXPORT vpCircleHoughTransform /** * \brief Class that gather the algorithm parameters. */ - class vpCircleHoughTransformParameters + class vpCircleHoughTransformParams { public: /** - * \brief Construct a new vpCircleHoughTransformParameters object with default parameters. + * \brief Construct a new vpCircleHoughTransformParams object with default parameters. */ - vpCircleHoughTransformParameters() + vpCircleHoughTransformParams() : m_filteringAndGradientType(vpImageFilter::CANNY_GBLUR_SOBEL_FILTERING) - , m_gaussianKernelSize(5) , m_gaussianStdev(1.f) - , m_gradientFilterKernelSize(3) , m_lowerCannyThresh(-1.f) , m_upperCannyThresh(-1.f) , m_edgeMapFilteringNbIter(1) @@ -88,8 +86,6 @@ class VISP_EXPORT vpCircleHoughTransform , m_centerYlimits(std::pair(std::numeric_limits::min(), std::numeric_limits::max())) , m_minRadius(0.f) , m_maxRadius(1000.f) - , m_dilatationKernelSize(3) - , m_averagingWindowSize(5) , m_centerMinThresh(50.f) , m_expectedNbCenters(-1) , m_circleProbaThresh(0.9f) @@ -99,11 +95,19 @@ class VISP_EXPORT vpCircleHoughTransform , m_centerMinDist(15.f) , m_mergingRadiusDiffThresh(1.5f * m_centerMinDist) { - + const unsigned int gaussianKernelSize_default = 5; + const unsigned int gradientFilterKernelSize_default = 3; + const unsigned int dilatationKernelSize_default = 3; + const unsigned int averagingWindowSize_default = 5; + + m_gaussianKernelSize = gaussianKernelSize_default; + m_gradientFilterKernelSize = gradientFilterKernelSize_default; + m_dilatationKernelSize = dilatationKernelSize_default; + m_averagingWindowSize = averagingWindowSize_default; } /** - * \brief Construct a new vpCircleHoughTransformParameters object. + * \brief Construct a new vpCircleHoughTransformParams object. * * \param[in] gaussianKernelSize Size of the Gaussian filter kernel used to smooth the input image. Must be an odd number. * \param[in] gaussianStdev Standard deviation of the Gaussian filter. @@ -140,7 +144,7 @@ class VISP_EXPORT vpCircleHoughTransform * \param[in] recordVotingPoints If true, the edge-map points having voted for each circle will be stored. * \param[in] visibilityRatioThresh Visibility threshold: which minimum ratio of the circle must be visible in order to keep a circle candidate. */ - vpCircleHoughTransformParameters( + vpCircleHoughTransformParams( const int &gaussianKernelSize , const float &gaussianStdev , const int &gradientFilterKernelSize @@ -441,12 +445,12 @@ class VISP_EXPORT vpCircleHoughTransform // // Configuration from files #ifdef VISP_HAVE_NLOHMANN_JSON /** - * \brief Create a new vpCircleHoughTransformParameters from a JSON file. + * \brief Create a new vpCircleHoughTransformParams from a JSON file. * * \param[in] jsonFile The path towards the JSON file. - * \return vpCircleHoughTransformParameters The corresponding vpCircleHoughTransformParameters object. + * \return vpCircleHoughTransformParams The corresponding vpCircleHoughTransformParams object. */ - inline static vpCircleHoughTransformParameters createFromJSON(const std::string &jsonFile) + inline static vpCircleHoughTransformParams createFromJSON(const std::string &jsonFile) { using json = nlohmann::json; @@ -468,7 +472,7 @@ class VISP_EXPORT vpCircleHoughTransform msg << "Byte position of error: " << e.byte; throw vpException(vpException::ioError, msg.str()); } - vpCircleHoughTransformParameters params = j; // Call from_json(const json& j, vpDetectorDNN& *this) to read json + vpCircleHoughTransformParams params = j; // Call from_json(const json& j, vpDetectorDNN& *this) to read json file.close(); return params; } @@ -485,7 +489,8 @@ class VISP_EXPORT vpCircleHoughTransform using json = nlohmann::json; std::ofstream file(jsonPath); const json j = *this; - file << j.dump(4); + const int indent = 4; + file << j.dump(indent); file.close(); } @@ -496,14 +501,15 @@ class VISP_EXPORT vpCircleHoughTransform * \param[in] j : The JSON object, resulting from the parsing of a JSON file. * \param[out] params : The circle Hough transform parameters that will be initialized from the JSON data. */ - friend inline void from_json(const nlohmann::json &j, vpCircleHoughTransformParameters ¶ms) + friend inline void from_json(const nlohmann::json &j, vpCircleHoughTransformParams ¶ms) { std::string filteringAndGradientName = vpImageFilter::vpCannyFiltAndGradTypeToStr(params.m_filteringAndGradientType); filteringAndGradientName = j.value("filteringAndGradientType", filteringAndGradientName); params.m_filteringAndGradientType = vpImageFilter::vpCannyFiltAndGradTypeFromStr(filteringAndGradientName); params.m_gaussianKernelSize = j.value("gaussianKernelSize", params.m_gaussianKernelSize); - if ((params.m_gaussianKernelSize % 2) != 1) { + const int checkEvenModulo = 2; + if ((params.m_gaussianKernelSize % checkEvenModulo) != 1) { throw vpException(vpException::badValue, "Gaussian Kernel size should be odd."); } @@ -513,7 +519,7 @@ class VISP_EXPORT vpCircleHoughTransform } params.m_gradientFilterKernelSize = j.value("gradientFilterKernelSize", params.m_gradientFilterKernelSize); - if ((params.m_gradientFilterKernelSize % 2) != 1) { + if ((params.m_gradientFilterKernelSize % checkEvenModulo) != 1) { throw vpException(vpException::badValue, "Gradient filter kernel (Sobel or Scharr) size should be odd."); } @@ -534,7 +540,7 @@ class VISP_EXPORT vpCircleHoughTransform params.m_dilatationKernelSize = j.value("dilatationKernelSize", params.m_dilatationKernelSize); params.m_averagingWindowSize = j.value("averagingWindowSize", params.m_averagingWindowSize); - if (params.m_averagingWindowSize <= 0 || (params.m_averagingWindowSize % 2) == 0) { + if ((params.m_averagingWindowSize <= 0) || ((params.m_averagingWindowSize % checkEvenModulo) == 0)) { throw vpException(vpException::badValue, "Averaging window size must be positive and odd."); } @@ -551,7 +557,7 @@ class VISP_EXPORT vpCircleHoughTransform params.m_circlePerfectness = j.value("circlePerfectnessThreshold", params.m_circlePerfectness); - if (params.m_circlePerfectness <= 0 || params.m_circlePerfectness > 1) { + if ((params.m_circlePerfectness <= 0) || (params.m_circlePerfectness > 1)) { throw vpException(vpException::badValue, "Circle perfectness must be in the interval ] 0; 1]."); } @@ -574,7 +580,7 @@ class VISP_EXPORT vpCircleHoughTransform * \param[out] j : A JSON parser object. * \param[in] params : The circle Hough transform parameters that will be serialized in the json object. */ - friend inline void to_json(nlohmann::json &j, const vpCircleHoughTransformParameters ¶ms) + friend inline void to_json(nlohmann::json &j, const vpCircleHoughTransformParams ¶ms) { std::pair radiusLimits = { params.m_minRadius, params.m_maxRadius }; @@ -657,17 +663,21 @@ class VISP_EXPORT vpCircleHoughTransform friend class vpCircleHoughTransform; }; - /** - * \brief Construct a new vpCircleHoughTransform object with default parameters. - */ +#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS + typedef vpCircleHoughTransformParams vpCircleHoughTransformParameters; +#endif + +/** + * \brief Construct a new vpCircleHoughTransform object with default parameters. + */ vpCircleHoughTransform(); /** * \brief Construct a new vpCircleHoughTransform object - * from a \b vpCircleHoughTransformParameters object. + * from a \b vpCircleHoughTransformParams object. * \param[in] algoParams The parameters of the Circle Hough Transform. */ - explicit vpCircleHoughTransform(const vpCircleHoughTransformParameters &algoParams); + explicit vpCircleHoughTransform(const vpCircleHoughTransformParams &algoParams); /** * \brief Destroy the vp Circle Hough Transform object @@ -739,7 +749,7 @@ class VISP_EXPORT vpCircleHoughTransform * does not exist. * \param[in] jsonPath The path towards the JSON configuration file. */ - vpCircleHoughTransform(const std::string &jsonPath); + explicit vpCircleHoughTransform(const std::string &jsonPath); /** * \brief Initialize all the algorithm parameters using the JSON file @@ -791,7 +801,7 @@ class VISP_EXPORT vpCircleHoughTransform * * \param[in] algoParams The algorithm parameters. */ - void init(const vpCircleHoughTransformParameters &algoParams); + void init(const vpCircleHoughTransformParams &algoParams); /** * \brief Permits to choose the filtering + gradient operators to use. @@ -817,7 +827,8 @@ class VISP_EXPORT vpCircleHoughTransform m_algoParams.m_gaussianKernelSize = kernelSize; m_algoParams.m_gaussianStdev = stdev; - if ((m_algoParams.m_gaussianKernelSize % 2) != 1) { + const unsigned int checkEvenModulo = 2; + if ((m_algoParams.m_gaussianKernelSize % checkEvenModulo) != 1) { throw vpException(vpException::badValue, "Gaussian Kernel size should be odd."); } @@ -837,7 +848,8 @@ class VISP_EXPORT vpCircleHoughTransform { m_algoParams.m_gradientFilterKernelSize = apertureSize; - if ((m_algoParams.m_gradientFilterKernelSize % 2) != 1) { + const unsigned int checkEvenModulo = 2; + if ((m_algoParams.m_gradientFilterKernelSize % checkEvenModulo) != 1) { throw vpException(vpException::badValue, "Gradient filter (Sobel or Scharr) Kernel size should be odd."); } @@ -972,10 +984,12 @@ class VISP_EXPORT vpCircleHoughTransform m_algoParams.m_averagingWindowSize = averagingWindowSize; m_algoParams.m_expectedNbCenters = expectedNbCenters; - if (m_algoParams.m_dilatationKernelSize < 3) { + const int minDilatationKernel = 3; + const unsigned int checkEvenModulo = 2; + if (m_algoParams.m_dilatationKernelSize < minDilatationKernel) { throw vpException(vpException::badValue, "Dilatation kernel size for center detection must be greater or equal to 3."); } - else if ((m_algoParams.m_dilatationKernelSize % 2) == 0) { + else if ((m_algoParams.m_dilatationKernelSize % checkEvenModulo) == 0) { throw vpException(vpException::badValue, "Dilatation kernel size for center detection must be odd."); } @@ -983,7 +997,7 @@ class VISP_EXPORT vpCircleHoughTransform throw vpException(vpException::badValue, "Votes thresholds for center detection must be positive."); } - if ((m_algoParams.m_averagingWindowSize <= 0) || ((m_algoParams.m_averagingWindowSize % 2) == 0)) { + if ((m_algoParams.m_averagingWindowSize <= 0) || ((m_algoParams.m_averagingWindowSize % checkEvenModulo) == 0)) { throw vpException(vpException::badValue, "Averaging window size must be positive and odd."); } } @@ -1251,7 +1265,7 @@ class VISP_EXPORT vpCircleHoughTransform virtual void mergeCandidates(std::vector &circleCandidates, std::vector &circleCandidatesVotes, std::vector &circleCandidatesProba, std::vector > > &votingPoints); - vpCircleHoughTransformParameters m_algoParams; /*!< Attributes containing all the algorithm parameters.*/ + vpCircleHoughTransformParams m_algoParams; /*!< Attributes containing all the algorithm parameters.*/ // // Gaussian smoothing attributes vpArray2D m_fg; diff --git a/modules/imgproc/include/visp3/imgproc/vpContours.h b/modules/imgproc/include/visp3/imgproc/vpContours.h index fa60f10c9e..90e1a5d54c 100644 --- a/modules/imgproc/include/visp3/imgproc/vpContours.h +++ b/modules/imgproc/include/visp3/imgproc/vpContours.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -67,8 +67,8 @@ \brief Basic contours extraction. */ -#ifndef _vpContours_h_ -#define _vpContours_h_ +#ifndef VP_CONTOURS_H +#define VP_CONTOURS_H #include #include @@ -96,8 +96,9 @@ typedef enum /*! * Direction object. */ -struct vpDirection +class vpDirection { +public: //! Direction vpDirectionType m_direction; @@ -112,25 +113,27 @@ struct vpDirection */ vpDirection() { + const unsigned int dir0 = 0, dir1 = 1, dir2 = 2, dir3 = 3; + const unsigned int dir4 = 4, dir5 = 5, dir6 = 6, dir7 = 7; m_direction = NORTH; - m_dirx[0] = 0; - m_dirx[1] = 1; - m_dirx[2] = 1; - m_dirx[3] = 1; - m_dirx[4] = 0; - m_dirx[5] = -1; - m_dirx[6] = -1; - m_dirx[7] = -1; - - m_diry[0] = -1; - m_diry[1] = -1; - m_diry[2] = 0; - m_diry[3] = 1; - m_diry[4] = 1; - m_diry[5] = 1; - m_diry[6] = 0; - m_diry[7] = -1; + m_dirx[dir0] = 0; + m_dirx[dir1] = 1; + m_dirx[dir2] = 1; + m_dirx[dir3] = 1; + m_dirx[dir4] = 0; + m_dirx[dir5] = -1; + m_dirx[dir6] = -1; + m_dirx[dir7] = -1; + + m_diry[dir0] = -1; + m_diry[dir1] = -1; + m_diry[dir2] = 0; + m_diry[dir3] = 1; + m_diry[dir4] = 1; + m_diry[dir5] = 1; + m_diry[dir6] = 0; + m_diry[dir7] = -1; } /*! diff --git a/modules/imgproc/include/visp3/imgproc/vpImgproc.h b/modules/imgproc/include/visp3/imgproc/vpImgproc.h index c476fc4608..e3b9b734f9 100644 --- a/modules/imgproc/include/visp3/imgproc/vpImgproc.h +++ b/modules/imgproc/include/visp3/imgproc/vpImgproc.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -37,8 +37,8 @@ */ -#ifndef _vpImgproc_h_ -#define _vpImgproc_h_ +#ifndef VP_IMGPROC_H +#define VP_IMGPROC_H #include #include diff --git a/modules/imgproc/src/vpCLAHE.cpp b/modules/imgproc/src/vpCLAHE.cpp index 1a9aeaa484..5eacf9add4 100644 --- a/modules/imgproc/src/vpCLAHE.cpp +++ b/modules/imgproc/src/vpCLAHE.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -203,39 +203,44 @@ float transferValue(int v, const std::vector &hist, std::vector &clipp return transferValue(v, clippedHist); } -void clahe(const vpImage &I1, vpImage &I2, int blockRadius, int bins, float slope, bool fast) +bool checkClaheInputs(const int &blockRadius, const int &bins, const unsigned int &width, const unsigned int &height) { if (blockRadius < 0) { std::cerr << "Error: blockRadius < 0!" << std::endl; - return; + return false; } - if ((bins < 0) || (bins > 256)) { - std::cerr << "Error: (bins < 0 || bins > 256)!" << std::endl; - return; + const int maxBins = 256; + if ((bins < 0) || (bins > maxBins)) { + std::cerr << "Error: (bins < 0 || bins > " << maxBins << ")!" << std::endl; + return false; } - if ((static_cast((2 * blockRadius) + 1) > I1.getWidth()) || (static_cast((2 * blockRadius) + 1) > I1.getHeight())) { + const int twice = 2; + if ((static_cast((twice * blockRadius) + 1) > width) || (static_cast((twice * blockRadius) + 1) > height)) { std::cerr << "Error: (unsigned int) (2*blockRadius+1) > I1.getWidth() || " "(unsigned int) (2*blockRadius+1) > I1.getHeight()!" << std::endl; - return; + return false; } + return true; +} - I2.resize(I1.getHeight(), I1.getWidth()); +void clahe(const vpImage &I1, vpImage &I2, int blockRadius, int bins, float slope, bool fast) +{ + if (!checkClaheInputs(blockRadius, bins, I1.getWidth(), I1.getHeight())) { return; } + I2.resize(I1.getHeight(), I1.getWidth()); if (fast) { - int blockSize = (2 * blockRadius) + 1; + const int val_2 = 2; + int blockSize = (val_2 * blockRadius) + 1; int limit = static_cast(((slope * blockSize * blockSize) / bins) + 0.5); - /* div */ int nc = I1.getWidth() / blockSize; int nr = I1.getHeight() / blockSize; - /* % */ int cm = I1.getWidth() - (nc * blockSize); std::vector cs; - switch (cm) { case 0: cs.resize(nc); @@ -243,7 +248,6 @@ void clahe(const vpImage &I1, vpImage &I2, int blo cs[i] = (i * blockSize) + blockRadius + 1; } break; - case 1: cs.resize(nc + 1); for (int i = 0; i < nc; ++i) { @@ -251,19 +255,17 @@ void clahe(const vpImage &I1, vpImage &I2, int blo } cs[nc] = I1.getWidth() - blockRadius - 1; break; - default: - cs.resize(nc + 2); + cs.resize(nc + val_2); cs[0] = blockRadius + 1; for (int i = 0; i < nc; ++i) { - cs[i + 1] = (i * blockSize) + blockRadius + 1 + (cm / 2); + cs[i + 1] = (i * blockSize) + blockRadius + 1 + (cm / val_2); } cs[nc + 1] = I1.getWidth() - blockRadius - 1; } int rm = I1.getHeight() - (nr * blockSize); std::vector rs; - switch (rm) { case 0: rs.resize(static_cast(nr)); @@ -271,7 +273,6 @@ void clahe(const vpImage &I1, vpImage &I2, int blo rs[i] = (i * blockSize) + blockRadius + 1; } break; - case 1: rs.resize(static_cast(nr + 1)); for (int i = 0; i < nr; ++i) { @@ -279,29 +280,22 @@ void clahe(const vpImage &I1, vpImage &I2, int blo } rs[nr] = I1.getHeight() - blockRadius - 1; break; - default: - rs.resize(static_cast(nr + 2)); + rs.resize(static_cast(nr + val_2)); rs[0] = blockRadius + 1; for (int i = 0; i < nr; ++i) { - rs[i + 1] = (i * blockSize) + blockRadius + 1 + (rm / 2); + rs[i + 1] = (i * blockSize) + blockRadius + 1 + (rm / val_2); } rs[nr + 1] = I1.getHeight() - blockRadius - 1; } - std::vector hist(static_cast(bins + 1)); - std::vector cdfs(static_cast(bins + 1)); - std::vector tl; - std::vector tr; - std::vector br; - std::vector bl; - + std::vector hist(static_cast(bins + 1)), cdfs(static_cast(bins + 1)); + std::vector tl, tr, br, bl; int rs_size = static_cast(rs.size()); for (int r = 0; r <= rs_size; ++r) { int r0 = std::max(0, r - 1); int r1 = std::min(static_cast(rs.size()) - 1, r); int dr = rs[r1] - rs[r0]; - createHistogram(blockRadius, bins, cs[0], rs[r0], I1, hist); tr = createTransfer(hist, limit, cdfs); if (r0 == r1) { @@ -314,16 +308,13 @@ void clahe(const vpImage &I1, vpImage &I2, int blo int yMin = (r == 0 ? 0 : rs[r0]); int yMax = (r < static_cast(rs.size()) ? rs[r1] : I1.getHeight()); - int cs_size = static_cast(cs.size()); for (int c = 0; c <= cs_size; ++c) { int c0 = std::max(0, c - 1); int c1 = std::min(static_cast(cs.size()) - 1, c); int dc = cs[c1] - cs[c0]; - tl = tr; bl = br; - if (c0 != c1) { createHistogram(blockRadius, bins, cs[c1], rs[r0], I1, hist); tr = createTransfer(hist, limit, cdfs); @@ -340,7 +331,6 @@ void clahe(const vpImage &I1, vpImage &I2, int blo int xMax = (c < static_cast(cs.size()) ? cs[c1] : I1.getWidth()); for (int y = yMin; y < yMax; ++y) { float wy = static_cast(rs[r1] - y) / dr; - for (int x = xMin; x < xMax; ++x) { float wx = static_cast(cs[c1] - x) / dc; int v = fastRound((I1[y][x] / 255.0f) * bins); @@ -348,47 +338,27 @@ void clahe(const vpImage &I1, vpImage &I2, int blo float t01 = tr[v]; float t10 = bl[v]; float t11 = br[v]; - float t0 = 0.0f, t1 = 0.0f; - - if (c0 == c1) { - t0 = t00; - t1 = t10; - } - else { - t0 = (wx * t00) + ((1.0f - wx) * t01); - t1 = (wx * t10) + ((1.0f - wx) * t11); - } - + float t0 = (c0 == c1) ? t00 : ((wx * t00) + ((1.0f - wx) * t01)); + float t1 = (c0 == c1) ? t10 : ((wx * t10) + ((1.0f - wx) * t11)); float t = (r0 == r1) ? t0 : ((wy * t0) + ((1.0f - wy) * t1)); - I2[y][x] = std::max(0, std::min(255, fastRound(t * 255.0f))); + const int maxPixelIntensity = 255; + I2[y][x] = std::max(0, std::min(maxPixelIntensity, fastRound(t * 255.0f))); } } } } } else { - std::vector hist(bins + 1), prev_hist(bins + 1); - std::vector clippedHist(bins + 1); - + std::vector hist(bins + 1), prev_hist(bins + 1), clippedHist(bins + 1); bool first = true; int xMin0 = 0; int xMax0 = std::min(static_cast(I1.getWidth()), blockRadius); - int i1_height = static_cast(I1.getHeight()); for (int y = 0; y < i1_height; ++y) { int yMin = std::max(0, y - static_cast(blockRadius)); int yMax = std::min(static_cast(I1.getHeight()), y + blockRadius + 1); int h = yMax - yMin; -#if 0 - std::fill(hist.begin(), hist.end(), 0); - // Compute histogram for the current block - for (int yi = yMin; yi < yMax; ++yi) { - for (int xi = xMin0; xi < xMax0; ++xi) { - ++hist[fastRound(I1[yi][xi] / 255.0f * bins)]; - } - } -#else if (first) { first = false; // Compute histogram for the block at (0,0) @@ -397,7 +367,7 @@ void clahe(const vpImage &I1, vpImage &I2, int blo ++hist[fastRound((I1[yi][xi] / 255.0f) * bins)]; } } - } + } else { hist = prev_hist; @@ -418,7 +388,7 @@ void clahe(const vpImage &I1, vpImage &I2, int blo } } prev_hist = hist; -#endif + int i1_width = static_cast(I1.getWidth()); for (int x = 0; x < i1_width; ++x) { int xMin = std::max(0, x - static_cast(blockRadius)); @@ -446,9 +416,9 @@ void clahe(const vpImage &I1, vpImage &I2, int blo int limit = static_cast(((slope * n) / bins) + 0.5f); I2[y][x] = fastRound(transferValue(v, hist, clippedHist, limit) * 255.0f); } - } } } +} void clahe(const vpImage &I1, vpImage &I2, int blockRadius, int bins, float slope, bool fast) { @@ -466,10 +436,11 @@ void clahe(const vpImage &I1, vpImage &I2, int blockRadius, int clahe(pG, resG, blockRadius, bins, slope, fast); clahe(pB, resB, blockRadius, bins, slope, fast); + const unsigned int sizeRGBa = 4; I2.resize(I1.getHeight(), I1.getWidth()); unsigned int size = I2.getWidth() * I2.getHeight(); unsigned char *ptrStart = reinterpret_cast(I2.bitmap); - unsigned char *ptrEnd = ptrStart + (size * 4); + unsigned char *ptrEnd = ptrStart + (size * sizeRGBa); unsigned char *ptrCurrent = ptrStart; unsigned int cpt = 0; diff --git a/modules/imgproc/src/vpCircleHoughTransform.cpp b/modules/imgproc/src/vpCircleHoughTransform.cpp index d146c1a146..0dadbcc925 100644 --- a/modules/imgproc/src/vpCircleHoughTransform.cpp +++ b/modules/imgproc/src/vpCircleHoughTransform.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -33,16 +33,12 @@ #include -#ifdef ENABLE_VISP_NAMESPACE -namespace visp -{ -#endif - -// Static variables -const unsigned char vpCircleHoughTransform::edgeMapOn = 255; -const unsigned char vpCircleHoughTransform::edgeMapOff = 0; +BEGIN_VISP_NAMESPACE #if (VISP_CXX_STANDARD == VISP_CXX_STANDARD_98) +namespace +{ + // Sorting by decreasing probabilities bool hasBetterProba(std::pair a, std::pair b) { @@ -95,8 +91,13 @@ scaleFilter(vpArray2D &filter, const float &scale) } } } +} #endif +// Static variables +const unsigned char vpCircleHoughTransform::edgeMapOn = 255; +const unsigned char vpCircleHoughTransform::edgeMapOff = 0; + vpCircleHoughTransform::vpCircleHoughTransform() : m_algoParams() , mp_mask(nullptr) @@ -105,7 +106,7 @@ vpCircleHoughTransform::vpCircleHoughTransform() initGradientFilters(); } -vpCircleHoughTransform::vpCircleHoughTransform(const vpCircleHoughTransformParameters &algoParams) +vpCircleHoughTransform::vpCircleHoughTransform(const vpCircleHoughTransformParams &algoParams) : m_algoParams(algoParams) , mp_mask(nullptr) { @@ -114,7 +115,7 @@ vpCircleHoughTransform::vpCircleHoughTransform(const vpCircleHoughTransformParam } void -vpCircleHoughTransform::init(const vpCircleHoughTransformParameters &algoParams) +vpCircleHoughTransform::init(const vpCircleHoughTransformParams &algoParams) { m_algoParams = algoParams; initGaussianFilters(); @@ -191,7 +192,8 @@ vpCircleHoughTransform::initGradientFilters() }; #endif - if ((m_algoParams.m_gradientFilterKernelSize % 2) != 1) { + 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); @@ -601,135 +603,132 @@ vpCircleHoughTransform::computeCenterCandidates() if (std::abs(mag) >= std::numeric_limits::epsilon()) { sx = m_dIx[r][c] / mag; sy = m_dIy[r][c] / mag; - } - else { - continue; - } - // 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); - } + // 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); - } + 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); - float deltar_y = 1.f / std::abs(sy); - float deltar = std::min(deltar_x, deltar_y); + float deltar_x = 1.f / std::abs(sx); + float deltar_y = 1.f / std::abs(sy); + float deltar = std::min(deltar_x, deltar_y); - float 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 + 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. - } + 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; - int y_low, y_high; + int x_low, x_high; + int 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 (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 (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 ((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); - } - }; + 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_low, y_low, + offsetX, offsetY, + accumulatorWidth, accumulatorHeight, + centersAccum, hasToStopLoop + ); + + updateAccumulator(x1, y1, x_high, y_high, + offsetX, offsetY, + accumulatorWidth, accumulatorHeight, + centersAccum, hasToStopLoop + ); + } - updateAccumulator(x1, y1, x_high, y_high, - offsetX, offsetY, - accumulatorWidth, accumulatorHeight, - centersAccum, hasToStopLoop - ); + sx = -sx; + sy = -sy; } - - sx = -sx; - sy = -sy; } } } @@ -898,7 +897,8 @@ vpCircleHoughTransform::computeCircleCandidates() #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98) for (auto edgePoint : m_edgePointsList) #else - for (size_t e = 0; e < m_edgePointsList.size(); ++e) + 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) @@ -1044,7 +1044,7 @@ vpCircleHoughTransform::computeCircleCandidates() ++idCandidate; } - if ((votes_effective > m_algoParams.m_centerMinThresh) && (votes_effective >= (m_algoParams.m_circleVisibilityRatioThresh * 2.f * M_PIf * r_effective))) { + if ((votes_effective > m_algoParams.m_centerMinThresh) && (votes_effective >= (m_algoParams.m_circleVisibilityRatioThresh * 2.f * M_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); @@ -1151,7 +1151,8 @@ vpCircleHoughTransform::mergeCandidates(std::vector &circleCandid vpImagePoint newCenter = ((cic_i.getCenter() * circleCandidatesProba[i]) + (cic_j.getCenter() * circleCandidatesProba[j])) / totalProba; cic_i = vpImageCircle(newCenter, newRadius); circleCandidates[j] = circleCandidates[nbCandidates - 1]; - circleCandidatesVotes[i] = totalVotes / 2; // Compute the mean vote + 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]; diff --git a/modules/imgproc/src/vpConnectedComponents.cpp b/modules/imgproc/src/vpConnectedComponents.cpp index 0eaff447f8..b8e8a96523 100644 --- a/modules/imgproc/src/vpConnectedComponents.cpp +++ b/modules/imgproc/src/vpConnectedComponents.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/imgproc/src/vpContours.cpp b/modules/imgproc/src/vpContours.cpp index aed9cc035e..3db6c0767b 100644 --- a/modules/imgproc/src/vpContours.cpp +++ b/modules/imgproc/src/vpContours.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -184,7 +184,8 @@ void followBorder(vpImage &I, const vpImagePoint &ij, vpImagePoint &i2j2, v i2j2 = i1j1; vpImagePoint i3j3 = ij; //(3.2) - bool checked[8] = { false, false, false, false, false, false, false, false }; + const int sizeChecked = 8; + bool checked[sizeChecked] = { false, false, false, false, false, false, false, false }; bool i4j4_eq_ij_and_i3j3_eq_i1j1 = false; while (i4j4_eq_ij_and_i3j3_eq_i1j1 == false) { @@ -196,7 +197,7 @@ void followBorder(vpImage &I, const vpImagePoint &ij, vpImagePoint &i2j2, v vpImagePoint i4j4(-1, -1); // Reset checked - for (int cpt = 0; cpt < 8; ++cpt) { + for (int cpt = 0; cpt < sizeChecked; ++cpt) { checked[cpt] = false; } diff --git a/modules/imgproc/src/vpFloodFill.cpp b/modules/imgproc/src/vpFloodFill.cpp index e89bbc6510..689f349f85 100644 --- a/modules/imgproc/src/vpFloodFill.cpp +++ b/modules/imgproc/src/vpFloodFill.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/imgproc/src/vpImgproc.cpp b/modules/imgproc/src/vpImgproc.cpp index 5a82a3ace3..4077523a47 100644 --- a/modules/imgproc/src/vpImgproc.cpp +++ b/modules/imgproc/src/vpImgproc.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -181,8 +181,9 @@ vpGammaColorHandling vpGammaColorHandlingFromString(const std::string &name) void adjust(vpImage &I, double alpha, double beta) { // Construct the look-up table - unsigned char lut[256]; - for (unsigned int i = 0; i < 256; ++i) { + const unsigned int lutSize = 256; + unsigned char lut[lutSize]; + for (unsigned int i = 0; i < lutSize; ++i) { lut[i] = vpMath::saturate((alpha * i) + beta); } @@ -201,8 +202,9 @@ void adjust(const vpImage &I1, vpImage &I2, double void adjust(vpImage &I, double alpha, double beta) { // Construct the look-up table - vpRGBa lut[256]; - for (unsigned int i = 0; i < 256; ++i) { + const unsigned int lutSize = 256; + vpRGBa lut[lutSize]; + for (unsigned int i = 0; i < lutSize; ++i) { lut[i].R = vpMath::saturate((alpha * i) + beta); lut[i].G = vpMath::saturate((alpha * i) + beta); lut[i].B = vpMath::saturate((alpha * i) + beta); @@ -359,15 +361,16 @@ void gammaCorrectionNonLinearMethod(vpImage &I, const vpImage(i); - float phi = (M_PIf * x) / (2.f * x_m); + float phi = (M_PI_FLOAT * x) / (2.f * x_m); float f1 = a * std::cos(phi); - float k = rho * std::sin((4 * M_PIf * x) / 255.f); + float k = rho * std::sin((4 * M_PI_FLOAT * x) / 255.f); float f2 = ((k + b)*std::cos(alpha)) + (x * std::sin(alpha)); float r = c * std::abs((x / x_m) - 1.f); - float f3 = r * std::cos((3.f * M_PIf * x) / 255.f); + float f3 = r * std::cos((3.f * M_PI_FLOAT * x) / 255.f); float g = f1 + f2 + f3; float gamma = 1 + g; float inverse_gamma = 1.f / gamma; @@ -396,7 +399,8 @@ void gammaCorrectionClassBasedMethod(vpImage &I, const vpImage (1./tau); - unsigned char lut[256]; + const unsigned int lutSize = 256; + unsigned char lut[lutSize]; float gamma = 0.f; if (isAlreadyHighContrast) { // Case medium to high contrast image @@ -407,13 +411,13 @@ void gammaCorrectionClassBasedMethod(vpImage &I, const vpImage= VISP_CXX_STANDARD_11) gamma = -static_cast(std::log2(stdevNormalized)); #else - gamma = -static_cast(std::log(stdevNormalized) / std::log(2)); + gamma = -static_cast(std::log(stdevNormalized) / std::log(2.f)); #endif } if (meanNormalized < 0.5) { // Case dark image float meanPowerGamma = static_cast(std::pow(meanNormalized, gamma)); - for (unsigned int i = 0; i <= 255; ++i) { + for (unsigned int i = 0; i < lutSize; ++i) { float iNormalized = static_cast(i)/255.f; float iPowerGamma = std::pow(iNormalized, gamma); lut[i] = vpMath::saturate(255.f * (iPowerGamma / (iPowerGamma + ((1.f - iPowerGamma) * meanPowerGamma)))); @@ -421,7 +425,7 @@ void gammaCorrectionClassBasedMethod(vpImage &I, const vpImage(i)/255.f; lut[i] = vpMath::saturate(std::pow(iNormalized, gamma) * 255.f); } @@ -463,9 +467,9 @@ void gammaCorrectionProbBasedMethod(vpImage &I, const vpImage(i)/255.f; @@ -486,16 +490,18 @@ void gammaCorrectionProbBasedMethod(vpImage &I, const vpImage &I, const vpImage *p_mask) { unsigned int width = I.getWidth(), height = I.getHeight(); + const unsigned int scale2 = 2, scale4 = 4, scale8 = 8; vpImage I_2, I_4, I_8; - I.subsample(2, 2, I_2); - I.subsample(4, 4, I_4); - I.subsample(8, 8, I_8); + I.subsample(scale2, scale2, I_2); + I.subsample(scale4, scale4, I_4); + I.subsample(scale8, scale8, I_8); vpImage I_blur, I_2_blur, I_4_blur, I_8_blur; const bool normalize = true; - vpImageFilter::gaussianBlur(I, I_blur, 3, 0.f, normalize, p_mask); - vpImageFilter::gaussianBlur(I_2, I_2_blur, 3, 0.f, normalize, p_mask); - vpImageFilter::gaussianBlur(I_4, I_4_blur, 3, 0.f, normalize, p_mask); - vpImageFilter::gaussianBlur(I_8, I_8_blur, 3, 0.f, normalize, p_mask); + const unsigned int gaussKernelSize = 3; + vpImageFilter::gaussianBlur(I, I_blur, gaussKernelSize, 0.f, normalize, p_mask); + vpImageFilter::gaussianBlur(I_2, I_2_blur, gaussKernelSize, 0.f, normalize, p_mask); + vpImageFilter::gaussianBlur(I_4, I_4_blur, gaussKernelSize, 0.f, normalize, p_mask); + vpImageFilter::gaussianBlur(I_8, I_8_blur, gaussKernelSize, 0.f, normalize, p_mask); vpImage L, L_2, L_4, L_8; vpImageTools::resize(I_blur, L, width, height, vpImageTools::INTERPOLATION_CUBIC); vpImageTools::resize(I_2_blur, L_2, width, height, vpImageTools::INTERPOLATION_CUBIC); @@ -505,10 +511,11 @@ void gammaCorrectionSpatialBased(vpImage &I, const vpImage unsigned int size = height * width; float stdev = static_cast(I.getStdev(p_mask)); float p; - if (stdev <= 40) { + const float stdevThresh1 = 40., stdevThresh2 = 80.; + if (stdev <= stdevThresh1) { p = 2.f; } - else if (stdev <= 80) { + else if (stdev <= stdevThresh2) { p = (-0.025f * stdev) + 3.f; } else { @@ -552,15 +559,17 @@ void gammaCorrectionSpatialBased(vpImage &I, const vpImage *p_mask I_gray.bitmap[i] = static_cast((0.299 * rgb.R) + (0.587 * rgb.G) + (0.114 * rgb.B)); } vpImage I_2, I_4, I_8; - I_gray.subsample(2, 2, I_2); - I_gray.subsample(4, 4, I_4); - I_gray.subsample(8, 8, I_8); + const unsigned int scale2 = 2, scale4 = 4, scale8 = 8; + I_gray.subsample(scale2, scale2, I_2); + I_gray.subsample(scale4, scale4, I_4); + I_gray.subsample(scale8, scale8, I_8); vpImage I_blur, I_2_blur, I_4_blur, I_8_blur; const bool normalize = true; - vpImageFilter::gaussianBlur(I_gray, I_blur, 3, 0.f, normalize, p_mask); - vpImageFilter::gaussianBlur(I_2, I_2_blur, 3, 0.f, normalize, p_mask); - vpImageFilter::gaussianBlur(I_4, I_4_blur, 3, 0.f, normalize, p_mask); - vpImageFilter::gaussianBlur(I_8, I_8_blur, 3, 0.f, normalize, p_mask); + const unsigned int gaussKernelSize = 3; + vpImageFilter::gaussianBlur(I_gray, I_blur, gaussKernelSize, 0.f, normalize, p_mask); + vpImageFilter::gaussianBlur(I_2, I_2_blur, gaussKernelSize, 0.f, normalize, p_mask); + vpImageFilter::gaussianBlur(I_4, I_4_blur, gaussKernelSize, 0.f, normalize, p_mask); + vpImageFilter::gaussianBlur(I_8, I_8_blur, gaussKernelSize, 0.f, normalize, p_mask); vpImage L, L_2, L_4, L_8; vpImageTools::resize(I_blur, L, width, height, vpImageTools::INTERPOLATION_CUBIC); vpImageTools::resize(I_2_blur, L_2, width, height, vpImageTools::INTERPOLATION_CUBIC); @@ -570,10 +579,11 @@ void gammaCorrectionSpatialBased(vpImage &I, const vpImage *p_mask float stdev = static_cast(I.getStdev(p_mask)); float p; - if (stdev <= 40) { + const float stdevThresh1 = 40., stdevThresh2 = 80.; + if (stdev <= stdevThresh1) { p = 2.f; } - else if (stdev <= 80) { + else if (stdev <= stdevThresh2) { p = (-0.025f * stdev) + 3.f; } else { @@ -605,8 +615,9 @@ void gammaCorrection(vpImage &I, const float &gamma, const vpGamm if ((gamma > 0) && (method == GAMMA_MANUAL)) { inverse_gamma = 1.0f / gamma; // Construct the look-up table - unsigned char lut[256]; - for (unsigned int i = 0; i < 256; ++i) { + const unsigned int lutSize = 256; + unsigned char lut[lutSize]; + for (unsigned int i = 0; i < lutSize; ++i) { lut[i] = vpMath::saturate(std::pow(static_cast(i) / 255.0, inverse_gamma) * 255.0); } @@ -712,10 +723,11 @@ void stretchContrast(vpImage &I) unsigned char range = max - min; // Construct the look-up table - unsigned char lut[256]; + const unsigned int lutSize = 256, maxVal = lutSize - 1; + unsigned char lut[lutSize]; if (range > 0) { for (unsigned int x = min; x <= max; ++x) { - lut[x] = (255 * (x - min)) / range; + lut[x] = (maxVal * (x - min)) / range; } } else { @@ -763,11 +775,12 @@ void stretchContrast(vpImage &I) max.A = maxChannel; // Construct the look-up table - vpRGBa lut[256]; + const unsigned int lutSize = 256, maxVal = lutSize - 1; + vpRGBa lut[lutSize]; unsigned char rangeR = max.R - min.R; if (rangeR > 0) { for (unsigned int x = min.R; x <= max.R; ++x) { - lut[x].R = (255 * (x - min.R)) / rangeR; + lut[x].R = (maxVal * (x - min.R)) / rangeR; } } else { @@ -777,7 +790,7 @@ void stretchContrast(vpImage &I) unsigned char rangeG = max.G - min.G; if (rangeG > 0) { for (unsigned int x = min.G; x <= max.G; ++x) { - lut[x].G = (255 * (x - min.G)) / rangeG; + lut[x].G = (maxVal * (x - min.G)) / rangeG; } } else { @@ -787,7 +800,7 @@ void stretchContrast(vpImage &I) unsigned char rangeB = max.B - min.B; if (rangeB > 0) { for (unsigned int x = min.B; x <= max.B; ++x) { - lut[x].B = (255 * (x - min.B)) / rangeB; + lut[x].B = (maxVal * (x - min.B)) / rangeB; } } else { @@ -797,7 +810,7 @@ void stretchContrast(vpImage &I) unsigned char rangeA = max.A - min.A; if (rangeA > 0) { for (unsigned int x = min.A; x <= max.A; ++x) { - lut[x].A = (255 * (x - min.A)) / rangeA; + lut[x].A = (maxVal * (x - min.A)) / rangeA; } } else { diff --git a/modules/imgproc/src/vpMorph.cpp b/modules/imgproc/src/vpMorph.cpp index 500563f593..1745d2c7db 100644 --- a/modules/imgproc/src/vpMorph.cpp +++ b/modules/imgproc/src/vpMorph.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -102,7 +102,8 @@ void fillHoles(vpImage &I } // Perform flood fill - floodFill(flood_fill_mask, vpImagePoint(0, 0), 0, 255); + const unsigned char newVal = 255; + floodFill(flood_fill_mask, vpImagePoint(0, 0), 0, newVal); // Get current mask vpImage mask(I.getHeight(), I.getWidth()); diff --git a/modules/imgproc/src/vpRetinex.cpp b/modules/imgproc/src/vpRetinex.cpp index 6df8890bcb..5ef60f418a 100644 --- a/modules/imgproc/src/vpRetinex.cpp +++ b/modules/imgproc/src/vpRetinex.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -88,19 +88,17 @@ #include #include +#define MAX_RETINEX_SCALES 8 namespace VISP_NAMESPACE_NAME { - -#define MAX_RETINEX_SCALES 8 - std::vector retinexScalesDistribution(int scaleDiv, int level, int scale) { std::vector scales(MAX_RETINEX_SCALES); - + const int val_2 = 2; if (scaleDiv == 1) { scales[0] = scale / 2.0; } - else if (scaleDiv == 2) { + else if (scaleDiv == val_2) { scales[0] = scale / 2.0; scales[1] = scale; } @@ -158,10 +156,13 @@ void MSRCR(vpImage &I, int v_scale, int scaleDiv, int level, double dyna if (kernelSize == -1) { // Compute the kernel size from the input image size kernelSize = static_cast(std::min(I.getWidth(), I.getHeight()) / 2.0); - kernelSize = (kernelSize - (kernelSize % 2)) + 1; + const int moduloForOddityCheck = 2; + kernelSize = (kernelSize - (kernelSize % moduloForOddityCheck)) + 1; } - for (int channel = 0; channel < 3; ++channel) { + const int nbChannels = 3; + const int id0 = 0, id1 = 1, id2 = 2; + for (int channel = 0; channel < nbChannels; ++channel) { doubleRGB[static_cast(channel)] = vpImage(I.getHeight(), I.getWidth()); doubleResRGB[static_cast(channel)] = vpImage(I.getHeight(), I.getWidth()); @@ -176,7 +177,7 @@ void MSRCR(vpImage &I, int v_scale, int scaleDiv, int level, double dyna doubleRGB[static_cast(channel)].bitmap[cpt] = I.bitmap[cpt].G + 1.0; break; - case 2: + case id2: doubleRGB[static_cast(channel)].bitmap[cpt] = I.bitmap[cpt].B + 1.0; break; @@ -200,17 +201,17 @@ void MSRCR(vpImage &I, int v_scale, int scaleDiv, int level, double dyna } } - std::vector dest(size * 3); + std::vector dest(size * nbChannels); const double gain = 1.0, alpha = 128.0, offset = 0.0; for (unsigned int cpt = 0; cpt < size; ++cpt) { double logl = std::log(static_cast(I.bitmap[cpt].R + I.bitmap[cpt].G + I.bitmap[cpt].B + 3.0)); - dest[cpt * 3] = (gain * (std::log(alpha * doubleRGB[0].bitmap[cpt]) - logl) * doubleResRGB[0].bitmap[cpt]) + offset; - dest[(cpt * 3) + 1] = - (gain * (std::log(alpha * doubleRGB[1].bitmap[cpt]) - logl) * doubleResRGB[1].bitmap[cpt]) + offset; - dest[(cpt * 3) + 2] = - (gain * (std::log(alpha * doubleRGB[2].bitmap[cpt]) - logl) * doubleResRGB[2].bitmap[cpt]) + offset; + dest[cpt * nbChannels] = (gain * (std::log(alpha * doubleRGB[id0].bitmap[cpt]) - logl) * doubleResRGB[id0].bitmap[cpt]) + offset; + dest[(cpt * nbChannels) + id1] = + (gain * (std::log(alpha * doubleRGB[id1].bitmap[cpt]) - logl) * doubleResRGB[id1].bitmap[cpt]) + offset; + dest[(cpt * nbChannels) + id2] = + (gain * (std::log(alpha * doubleRGB[id2].bitmap[cpt]) - logl) * doubleResRGB[id2].bitmap[cpt]) + offset; } double sum = std::accumulate(dest.begin(), dest.end(), 0.0); @@ -236,23 +237,25 @@ void MSRCR(vpImage &I, int v_scale, int scaleDiv, int level, double dyna } for (unsigned int cpt = 0; cpt < size; ++cpt) { - I.bitmap[cpt].R = vpMath::saturate((255.0 * (dest[(cpt * 3) + 0] - mini)) / range); - I.bitmap[cpt].G = vpMath::saturate((255.0 * (dest[(cpt * 3) + 1] - mini)) / range); - I.bitmap[cpt].B = vpMath::saturate((255.0 * (dest[(cpt * 3) + 2] - mini)) / range); + I.bitmap[cpt].R = vpMath::saturate((255.0 * (dest[(cpt * nbChannels) + id0] - mini)) / range); + I.bitmap[cpt].G = vpMath::saturate((255.0 * (dest[(cpt * nbChannels) + id1] - mini)) / range); + I.bitmap[cpt].B = vpMath::saturate((255.0 * (dest[(cpt * nbChannels) + id2] - mini)) / range); } } void retinex(vpImage &I, int scale, int scaleDiv, int level, const double dynamic, int kernelSize) { // Assert scale - if ((scale < 16) || (scale > 250)) { - std::cerr << "Scale must be between the interval [16 - 250]" << std::endl; + const int minScale = 16, maxScale = 250; + if ((scale < minScale) || (scale > maxScale)) { + std::cerr << "Scale must be between the interval [" << minScale << " - " << maxScale << "]" << std::endl; return; } // Assert scaleDiv - if ((scaleDiv < 1) || (scaleDiv > 8)) { - std::cerr << "Scale division must be between the interval [1 - 8]" << std::endl; + const int minScaleDiv = 1, maxScaleDiv = 8; + if ((scaleDiv < minScaleDiv) || (scaleDiv > maxScaleDiv)) { + std::cerr << "Scale division must be between the interval [" << minScaleDiv << " - " << maxScaleDiv << "]" << std::endl; return; } diff --git a/modules/imgproc/src/vpThreshold.cpp b/modules/imgproc/src/vpThreshold.cpp index d8365d0f22..7b47c060a9 100644 --- a/modules/imgproc/src/vpThreshold.cpp +++ b/modules/imgproc/src/vpThreshold.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -46,19 +46,19 @@ namespace VISP_NAMESPACE_NAME bool isBimodal(const std::vector &hist_float) { int modes = 0; - + const int bimodalVal = 2; size_t hist_float_size_m_1 = hist_float.size() - 1; for (size_t cpt = 1; cpt < hist_float_size_m_1; ++cpt) { if ((hist_float[cpt - 1] < hist_float[cpt]) && (hist_float[cpt] > hist_float[cpt + 1])) { ++modes; } - if (modes > 2) { + if (modes > bimodalVal) { return false; } } - return (modes == 2); + return (modes == bimodalVal); } int computeThresholdHuang(const vpHistogram &hist) @@ -135,7 +135,8 @@ int computeThresholdHuang(const vpHistogram &hist) int computeThresholdIntermodes(const vpHistogram &hist) { - if (hist.getSize() < 3) { + const unsigned int minSize = 3; + if (hist.getSize() < minSize) { return -1; } @@ -160,23 +161,26 @@ int computeThresholdIntermodes(const vpHistogram &hist) } int iter = 0; + const float nbPtsAverage = 3.; + const int maxIter = 10000; while (!isBimodal(hist_float)) { // Smooth with a 3 point running mean filter size_t hist_float_size_m_1 = hist_float.size() - 1; for (size_t cpt = 1; cpt < hist_float_size_m_1; ++cpt) { - hist_float[cpt] = (hist_float[cpt - 1] + hist_float[cpt] + hist_float[cpt + 1]) / 3; + hist_float[cpt] = (hist_float[cpt - 1] + hist_float[cpt] + hist_float[cpt + 1]) / nbPtsAverage; } // First value hist_float[0] = (hist_float[0] + hist_float[1]) / 2.0f; // Last value - hist_float[hist_float.size() - 1] = (((hist_float.size() - 2) + hist_float.size()) - 1) / 2.0f; + const size_t var2 = 2; + hist_float[hist_float.size() - 1] = (((hist_float.size() - var2) + hist_float.size()) - 1) / 2.0f; ++iter; - if (iter > 10000) { - std::cerr << "Intermodes Threshold not found after 10000 iterations!" << std::endl; + if (iter > maxIter) { + std::cerr << "Intermodes Threshold not found after " << maxIter << " iterations!" << std::endl; return -1; } } @@ -220,7 +224,8 @@ int computeThresholdIsoData(const vpHistogram &hist, unsigned int imageSize) //% STEP 3 to n: repeat step 2 if T(i)~=T(i-1) while (std::abs(T2 - T) >= 1) { - MBT = sum_ip[static_cast(T2 - 2)] / cumsum[static_cast(T2 - 2)]; + const int val_2 = 2; + MBT = sum_ip[static_cast(T2 - val_2)] / cumsum[static_cast(T2 - val_2)]; MAT = (sum_ip.back() - sum_ip[static_cast(T2 - 1)]) / (cumsum.back() - cumsum[static_cast(T2 - 1)]); T = T2; @@ -267,7 +272,8 @@ int computeThresholdOtsu(const vpHistogram &hist, unsigned int imageSize) bool w_f_eq_nul = false; int cpt = 0; - while ((cpt < 256) && (!w_f_eq_nul)) { + const int maxCpt = 256; + while ((cpt < maxCpt) && (!w_f_eq_nul)) { w_B += hist[cpt]; bool w_b_eq_nul = vpMath::nul(w_B, std::numeric_limits::epsilon()); if (!w_b_eq_nul) { @@ -412,9 +418,10 @@ unsigned char autoThreshold(vpImage &I, const vpAutoThresholdMeth break; } + const unsigned char threshold2 = 255; if (threshold != -1) { // Threshold - vpImageTools::binarise(I, static_cast(threshold), static_cast(255), backgroundValue, foregroundValue, + vpImageTools::binarise(I, static_cast(threshold), threshold2, backgroundValue, foregroundValue, foregroundValue); } diff --git a/modules/io/include/visp3/io/vpDiskGrabber.h b/modules/io/include/visp3/io/vpDiskGrabber.h index fc2f9a275d..25366d9c6e 100644 --- a/modules/io/include/visp3/io/vpDiskGrabber.h +++ b/modules/io/include/visp3/io/vpDiskGrabber.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -35,12 +35,11 @@ * \file vpDiskGrabber.h * \brief Class to load image sequence from the disk. */ -#ifndef vpDiskGrabber_hh -#define vpDiskGrabber_hh +#ifndef VP_DISK_GRABBER_H +#define VP_DISK_GRABBER_H #include -#include #include #include #include diff --git a/modules/io/include/visp3/io/vpImageIo.h b/modules/io/include/visp3/io/vpImageIo.h index 2553761649..25a835c449 100644 --- a/modules/io/include/visp3/io/vpImageIo.h +++ b/modules/io/include/visp3/io/vpImageIo.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,10 +36,9 @@ \brief Read/write images */ -#ifndef _vpImageIo_h_ -#define _vpImageIo_h_ +#ifndef VP_IMAGE_IO_H +#define VP_IMAGE_IO_H -#include #include #include diff --git a/modules/io/include/visp3/io/vpJsonArgumentParser.h b/modules/io/include/visp3/io/vpJsonArgumentParser.h index 64ad2b9f50..9a3db027b2 100644 --- a/modules/io/include/visp3/io/vpJsonArgumentParser.h +++ b/modules/io/include/visp3/io/vpJsonArgumentParser.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,8 +31,8 @@ * An argument parser that can both use JSON files and command line arguments as inputs. */ -#ifndef _vpJsonArgumentParser_h_ -#define _vpJsonArgumentParser_h_ +#ifndef VP_JSON_ARGUMENT_PARSER_H +#define VP_JSON_ARGUMENT_PARSER_H #include @@ -181,7 +181,7 @@ class VISP_EXPORT vpJsonArgumentParser template vpJsonArgumentParser &addArgument(const std::string &name, T ¶meter, const bool required = true, const std::string &help = "No description") { - const auto getter = [name, this](nlohmann::json &j, bool create) -> nlohmann::json *{ + const auto getter = [name, this](nlohmann::json &j, bool create) -> nlohmann::json * { size_t pos = 0; nlohmann::json *f = &j; std::string token; diff --git a/modules/io/include/visp3/io/vpParseArgv.h b/modules/io/include/visp3/io/vpParseArgv.h index e93e2c2832..a9952b5615 100644 --- a/modules/io/include/visp3/io/vpParseArgv.h +++ b/modules/io/include/visp3/io/vpParseArgv.h @@ -23,8 +23,8 @@ \brief Command line argument parsing. */ -#ifndef vpParseArgv_h -#define vpParseArgv_h +#ifndef VP_PARSE_ARGV_H +#define VP_PARSE_ARGV_H #include #include diff --git a/modules/io/include/visp3/io/vpVideoReader.h b/modules/io/include/visp3/io/vpVideoReader.h index dad5010101..6886e37cde 100644 --- a/modules/io/include/visp3/io/vpVideoReader.h +++ b/modules/io/include/visp3/io/vpVideoReader.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,8 +36,8 @@ \brief Read videos and image sequences */ -#ifndef _vpVideoReader_h_ -#define _vpVideoReader_h_ +#ifndef VP_VIDEO_READER_H +#define VP_VIDEO_READER_H #include diff --git a/modules/io/include/visp3/io/vpVideoWriter.h b/modules/io/include/visp3/io/vpVideoWriter.h index 3e8e5d05ed..d17225037d 100644 --- a/modules/io/include/visp3/io/vpVideoWriter.h +++ b/modules/io/include/visp3/io/vpVideoWriter.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,8 +36,8 @@ \brief Write videos and sequences of images. */ -#ifndef _vpVideoWriter_h_ -#define _vpVideoWriter_h_ +#ifndef VP_VIDEO_WRITER_H +#define VP_VIDEO_WRITER_H #include diff --git a/modules/io/src/image/private/vpImageIoBackend.h b/modules/io/src/image/private/vpImageIoBackend.h index 3cce610e29..21e4de61f5 100644 --- a/modules/io/src/image/private/vpImageIoBackend.h +++ b/modules/io/src/image/private/vpImageIoBackend.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/io/src/image/private/vpImageIoLibjpeg.cpp b/modules/io/src/image/private/vpImageIoLibjpeg.cpp index 29ea3cc599..ef607de5a7 100644 --- a/modules/io/src/image/private/vpImageIoLibjpeg.cpp +++ b/modules/io/src/image/private/vpImageIoLibjpeg.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/io/src/image/private/vpImageIoLibpng.cpp b/modules/io/src/image/private/vpImageIoLibpng.cpp index e5c2aae071..2e7c4d6138 100644 --- a/modules/io/src/image/private/vpImageIoLibpng.cpp +++ b/modules/io/src/image/private/vpImageIoLibpng.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -76,7 +76,6 @@ void writePNGLibpng(const vpImage &I, const std::string &filename png_structp png_ptr = png_create_write_struct(PNG_LIBPNG_VER_STRING, nullptr, nullptr, nullptr); if (!png_ptr) { fclose(file); - vpERROR_TRACE("Error during png_create_write_struct()\n"); throw(vpImageException(vpImageException::ioError, "PNG write error")); } @@ -84,7 +83,6 @@ void writePNGLibpng(const vpImage &I, const std::string &filename if (!info_ptr) { fclose(file); png_destroy_write_struct(&png_ptr, nullptr); - vpERROR_TRACE("Error during png_create_info_struct()\n"); throw(vpImageException(vpImageException::ioError, "PNG write error")); } @@ -93,7 +91,6 @@ void writePNGLibpng(const vpImage &I, const std::string &filename if (setjmp(png_jmpbuf(png_ptr))) { fclose(file); png_destroy_write_struct(&png_ptr, &info_ptr); - vpERROR_TRACE("Error during init_io\n"); throw(vpImageException(vpImageException::ioError, "PNG write error")); } @@ -110,7 +107,6 @@ void writePNGLibpng(const vpImage &I, const std::string &filename if (setjmp(png_jmpbuf(png_ptr))) { fclose(file); png_destroy_write_struct(&png_ptr, &info_ptr); - vpERROR_TRACE("Error during write header\n"); throw(vpImageException(vpImageException::ioError, "PNG write error")); } @@ -173,7 +169,6 @@ void writePNGLibpng(const vpImage &I, const std::string &filename) png_structp png_ptr = png_create_write_struct(PNG_LIBPNG_VER_STRING, nullptr, nullptr, nullptr); if (!png_ptr) { fclose(file); - vpERROR_TRACE("Error during png_create_write_struct()\n"); throw(vpImageException(vpImageException::ioError, "PNG write error")); } @@ -181,7 +176,6 @@ void writePNGLibpng(const vpImage &I, const std::string &filename) if (!info_ptr) { fclose(file); png_destroy_write_struct(&png_ptr, nullptr); - vpERROR_TRACE("Error during png_create_info_struct()\n"); throw(vpImageException(vpImageException::ioError, "PNG write error")); } @@ -190,7 +184,6 @@ void writePNGLibpng(const vpImage &I, const std::string &filename) if (setjmp(png_jmpbuf(png_ptr))) { fclose(file); png_destroy_write_struct(&png_ptr, &info_ptr); - vpERROR_TRACE("Error during init_io\n"); throw(vpImageException(vpImageException::ioError, "PNG write error")); } @@ -207,7 +200,6 @@ void writePNGLibpng(const vpImage &I, const std::string &filename) if (setjmp(png_jmpbuf(png_ptr))) { fclose(file); png_destroy_write_struct(&png_ptr, &info_ptr); - vpERROR_TRACE("Error during write header\n"); throw(vpImageException(vpImageException::ioError, "PNG write error")); } @@ -315,7 +307,6 @@ void readPNGLibpng(vpImage &I, const std::string &filename) if (setjmp(png_jmpbuf(png_ptr))) { fclose(file); png_destroy_read_struct(&png_ptr, &info_ptr, nullptr); - vpERROR_TRACE("Error during init io\n"); throw(vpImageException(vpImageException::ioError, "PNG read error")); } @@ -474,7 +465,6 @@ void readPNGLibpng(vpImage &I, const std::string &filename) png_structp png_ptr = png_create_read_struct(PNG_LIBPNG_VER_STRING, nullptr, nullptr, nullptr); if (!png_ptr) { fclose(file); - vpERROR_TRACE("Error during png_create_read_struct()\n"); throw(vpImageException(vpImageException::ioError, "PNG read error")); } @@ -483,7 +473,6 @@ void readPNGLibpng(vpImage &I, const std::string &filename) if (!info_ptr) { fclose(file); png_destroy_read_struct(&png_ptr, nullptr, nullptr); - vpERROR_TRACE("Error during png_create_info_struct()\n"); throw(vpImageException(vpImageException::ioError, "PNG read error")); } @@ -492,7 +481,6 @@ void readPNGLibpng(vpImage &I, const std::string &filename) if (setjmp(png_jmpbuf(png_ptr))) { fclose(file); png_destroy_read_struct(&png_ptr, &info_ptr, nullptr); - vpERROR_TRACE("Error during init io\n"); throw(vpImageException(vpImageException::ioError, "PNG read error")); } diff --git a/modules/io/src/image/private/vpImageIoOpenCV.cpp b/modules/io/src/image/private/vpImageIoOpenCV.cpp index 8fc3d9e7d5..9b3641dce8 100644 --- a/modules/io/src/image/private/vpImageIoOpenCV.cpp +++ b/modules/io/src/image/private/vpImageIoOpenCV.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/io/src/image/private/vpImageIoPortable.cpp b/modules/io/src/image/private/vpImageIoPortable.cpp index f00e6b56ec..64037a4cf2 100644 --- a/modules/io/src/image/private/vpImageIoPortable.cpp +++ b/modules/io/src/image/private/vpImageIoPortable.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -53,8 +53,8 @@ BEGIN_VISP_NAMESPACE * \param h[out] : Image height. * \param maxval[out] : Maximum pixel value. */ -void vp_decodeHeaderPNM(const std::string &filename, std::ifstream &fd, const std::string &magic, unsigned int &w, - unsigned int &h, unsigned int &maxval) + void vp_decodeHeaderPNM(const std::string &filename, std::ifstream &fd, const std::string &magic, unsigned int &w, + unsigned int &h, unsigned int &maxval) { std::string line; unsigned int nb_elt = 4, cpt_elt = 0; @@ -81,7 +81,7 @@ void vp_decodeHeaderPNM(const std::string &filename, std::ifstream &fd, const st throw(vpImageException(vpImageException::ioError, "\"%s\" is not a PNM file with magic number %s", filename.c_str(), magic.c_str())); } - cpt_elt++; + ++cpt_elt; header.erase(header.begin(), header.begin() + 1); // erase first element that is processed } @@ -89,21 +89,21 @@ void vp_decodeHeaderPNM(const std::string &filename, std::ifstream &fd, const st if (cpt_elt == 1) { // decode width std::istringstream ss(header[0]); ss >> w; - cpt_elt++; + ++cpt_elt; header.erase(header.begin(), header.begin() + 1); // erase first element that is processed } else if (cpt_elt == 2) { // decode height std::istringstream ss(header[0]); ss >> h; - cpt_elt++; + ++cpt_elt; header.erase(header.begin(), header.begin() + 1); // erase first element that is processed } else if (cpt_elt == 3) { // decode maxval std::istringstream ss(header[0]); ss >> maxval; - cpt_elt++; + ++cpt_elt; header.erase(header.begin(), header.begin() + 1); // erase first element that is processed } @@ -141,7 +141,7 @@ void vp_decodeHeaderPFM(const std::string &filename, std::ifstream &fd, std::str throw(vpImageException(vpImageException::ioError, "\"%s\" is not a PFM file with PF (RGB) or Pf (gray) magic number", filename.c_str())); } - cpt_elt++; + ++cpt_elt; header.erase(header.begin(), header.begin() + 1); // erase first element that is processed } @@ -149,14 +149,14 @@ void vp_decodeHeaderPFM(const std::string &filename, std::ifstream &fd, std::str if (cpt_elt == 1) { // decode width std::istringstream ss(header[0]); ss >> w; - cpt_elt++; + ++cpt_elt; header.erase(header.begin(), header.begin() + 1); // erase first element that is processed } else if (cpt_elt == 2) { // decode height std::istringstream ss(header[0]); ss >> h; - cpt_elt++; + ++cpt_elt; header.erase(header.begin(), header.begin() + 1); // erase first element that is processed } @@ -164,7 +164,7 @@ void vp_decodeHeaderPFM(const std::string &filename, std::ifstream &fd, std::str std::istringstream ss(header[0]); ss >> scale; littleEndian = scale < 0; - cpt_elt++; + ++cpt_elt; header.erase(header.begin(), header.begin() + 1); // erase first element that is processed } diff --git a/modules/io/src/image/private/vpImageIoSimd.cpp b/modules/io/src/image/private/vpImageIoSimd.cpp index dce0cb3f90..4a181c61c1 100644 --- a/modules/io/src/image/private/vpImageIoSimd.cpp +++ b/modules/io/src/image/private/vpImageIoSimd.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/io/src/image/private/vpImageIoStb.cpp b/modules/io/src/image/private/vpImageIoStb.cpp index c755992af3..6be8c8f37b 100644 --- a/modules/io/src/image/private/vpImageIoStb.cpp +++ b/modules/io/src/image/private/vpImageIoStb.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/io/src/image/private/vpImageIoTinyEXR.cpp b/modules/io/src/image/private/vpImageIoTinyEXR.cpp index 7aa7a7a03b..f9ae329ce7 100644 --- a/modules/io/src/image/private/vpImageIoTinyEXR.cpp +++ b/modules/io/src/image/private/vpImageIoTinyEXR.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/io/src/image/vpImageIo.cpp b/modules/io/src/image/vpImageIo.cpp index e66dcc64fe..431effc3fe 100644 --- a/modules/io/src/image/vpImageIo.cpp +++ b/modules/io/src/image/vpImageIo.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/io/src/tools/vpJsonArgumentParser.cpp b/modules/io/src/tools/vpJsonArgumentParser.cpp index f38e0cbd91..bfe68f06c2 100644 --- a/modules/io/src/tools/vpJsonArgumentParser.cpp +++ b/modules/io/src/tools/vpJsonArgumentParser.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -121,28 +121,31 @@ void vpJsonArgumentParser::parse(int argc, const char *argv[]) // Parse command line arguments for (int i = 1; i < argc; ++i) { const std::string arg = argv[i]; + bool stop_for_loop = false; if (std::find(ignoredArguments.begin(), ignoredArguments.end(), i) != ignoredArguments.end()) { - continue; - } - if (arg == "-h" || arg == "--help") { - std::cout << help() << std::endl; - exit(1); + stop_for_loop = true; } + if (!stop_for_loop) { + if (arg == "-h" || arg == "--help") { + std::cout << help() << std::endl; + exit(1); + } - if (parsers.find(arg) != parsers.end()) { - if (i < argc - 1) { - updaters[arg](j, std::string(argv[i + 1])); - ++i; + if (parsers.find(arg) != parsers.end()) { + if (i < argc - 1) { + updaters[arg](j, std::string(argv[i + 1])); + ++i; + } + else { + std::stringstream ss; + ss << "Argument " << arg << " was passed but no value was provided" << std::endl; + throw vpException(vpException::ioError, ss.str()); + } } else { - std::stringstream ss; - ss << "Argument " << arg << " was passed but no value was provided" << std::endl; - throw vpException(vpException::ioError, ss.str()); + std::cerr << "Unknown parameter when parsing: " << arg << std::endl; } } - else { - std::cerr << "Unknown parameter when parsing: " << arg << std::endl; - } } // Get the values from json document and store them in the arguments passed by ref in addArgument diff --git a/modules/io/src/tools/vpParseArgv.cpp b/modules/io/src/tools/vpParseArgv.cpp index 075d03e983..e28f8447e5 100644 --- a/modules/io/src/tools/vpParseArgv.cpp +++ b/modules/io/src/tools/vpParseArgv.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * This file contains a procedure that handles table-based * argv-argc parsing. * @@ -39,9 +38,9 @@ BEGIN_VISP_NAMESPACE * in every application. */ -vpParseArgv::vpArgvInfo vpParseArgv::defaultTable[2] = { - {"-help", ARGV_HELP, (char *)nullptr, (char *)nullptr, "Print summary of command-line options and abort.\n"}, - {nullptr, ARGV_END, (char *)nullptr, (char *)nullptr, (char *)nullptr} }; + vpParseArgv::vpArgvInfo vpParseArgv::defaultTable[2] = { + {"-help", ARGV_HELP, (char *)nullptr, (char *)nullptr, "Print summary of command-line options and abort.\n"}, + {nullptr, ARGV_END, (char *)nullptr, (char *)nullptr, (char *)nullptr} }; int (*handlerProc1)(const char *dst, const char *key, const char *argument); int (*handlerProc2)(const char *dst, const char *key, int valargc, const char **argument); @@ -124,24 +123,28 @@ bool vpParseArgv::parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, i infoPtr = defaultTable; } for (; infoPtr->type != ARGV_END; ++infoPtr) { - if (infoPtr->key == nullptr) { - continue; - } - if ((infoPtr->key[1] != c) || (strncmp(infoPtr->key, curArg, length) != 0)) { - continue; - } - if (infoPtr->key[length] == 0) { - matchPtr = infoPtr; - goto gotMatch; - } - if (flags & ARGV_NO_ABBREV) { - continue; - } - if (matchPtr != nullptr) { - FPRINTF(stderr, "ambiguous option \"%s\"\n", curArg); - return true; + if (infoPtr->key != nullptr) { + bool stop_for_loop = false; + if ((infoPtr->key[1] != c) || (strncmp(infoPtr->key, curArg, length) != 0)) { + stop_for_loop = true;; + } + if (!stop_for_loop) { + if (infoPtr->key[length] == 0) { + matchPtr = infoPtr; + goto gotMatch; + } + if (flags & ARGV_NO_ABBREV) { + stop_for_loop = true; + } + if (!stop_for_loop) { + if (matchPtr != nullptr) { + FPRINTF(stderr, "ambiguous option \"%s\"\n", curArg); + return true; + } + matchPtr = infoPtr; + } + } } - matchPtr = infoPtr; } } if (matchPtr == nullptr) { diff --git a/modules/io/src/video/vpDiskGrabber.cpp b/modules/io/src/video/vpDiskGrabber.cpp index 1e5a84653b..1077987681 100644 --- a/modules/io/src/video/vpDiskGrabber.cpp +++ b/modules/io/src/video/vpDiskGrabber.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/io/src/video/vpVideoReader.cpp b/modules/io/src/video/vpVideoReader.cpp index 4f94da9773..c96bf97e16 100644 --- a/modules/io/src/video/vpVideoReader.cpp +++ b/modules/io/src/video/vpVideoReader.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,7 +36,6 @@ * \brief Read videos and image sequences */ -#include #include #include @@ -155,8 +154,6 @@ void vpVideoReader::getProperties() #endif } else if (m_formatType == FORMAT_UNKNOWN) { - // vpERROR_TRACE("The format of the file does not correspond to a readable - // format."); throw(vpException(vpException::fatalError, "The format of the file does " "not correspond to a readable " "format supported by ViSP.")); @@ -273,7 +270,7 @@ void vpVideoReader::acquire(vpImage &I) else { m_capture >> m_frame; if (m_frameStep == 1) { - m_frameCount++; + ++m_frameCount; } else { #if VISP_HAVE_OPENCV_VERSION >= 0x030000 @@ -368,7 +365,7 @@ void vpVideoReader::acquire(vpImage &I) else { m_capture >> m_frame; if (m_frameStep == 1) { - m_frameCount++; + ++m_frameCount; } else { #if VISP_HAVE_OPENCV_VERSION >= 0x030000 @@ -450,7 +447,7 @@ bool vpVideoReader::getFrame(vpImage &I, long frame_index) } } catch (...) { - vpERROR_TRACE("Couldn't find the %u th frame", frame_index); + // Couldn't find the %u th frame", frame_index return false; } } @@ -458,7 +455,7 @@ bool vpVideoReader::getFrame(vpImage &I, long frame_index) #if defined(HAVE_OPENCV_HIGHGUI) && defined(HAVE_OPENCV_VIDEOIO) #if (VISP_HAVE_OPENCV_VERSION >= 0x030000) if (!m_capture.set(cv::CAP_PROP_POS_FRAMES, frame_index)) { - vpERROR_TRACE("Couldn't find the %ld th frame", frame_index); + // Couldn't find the %ld th frame", frame_index return false; } @@ -480,7 +477,7 @@ bool vpVideoReader::getFrame(vpImage &I, long frame_index) vpImageConvert::convert(m_frame, I); #else if (!m_capture.set(CV_CAP_PROP_POS_FRAMES, frame_index)) { - vpERROR_TRACE("Couldn't find the %ld th frame", frame_index); + // Couldn't find the %ld th frame", frame_index return false; } @@ -527,7 +524,7 @@ bool vpVideoReader::getFrame(vpImage &I, long frame_index) } } catch (...) { - vpERROR_TRACE("Couldn't find the %u th frame", frame_index); + // Couldn't find the %u th frame", frame_index return false; } } @@ -535,7 +532,7 @@ bool vpVideoReader::getFrame(vpImage &I, long frame_index) #if defined(HAVE_OPENCV_HIGHGUI) && defined(HAVE_OPENCV_VIDEOIO) #if VISP_HAVE_OPENCV_VERSION >= 0x030000 if (!m_capture.set(cv::CAP_PROP_POS_FRAMES, frame_index)) { - vpERROR_TRACE("Couldn't find the %ld th frame", frame_index); + // Couldn't find the %ld th frame", frame_index return false; } m_capture >> m_frame; @@ -555,8 +552,7 @@ bool vpVideoReader::getFrame(vpImage &I, long frame_index) } #else if (!m_capture.set(CV_CAP_PROP_POS_FRAMES, frame_index)) { - vpERROR_TRACE("Couldn't find the %ld th frame", - frame_index); // next index + // Couldn't find the %ld th frame", frame_index return false; } m_capture >> m_frame; @@ -699,8 +695,7 @@ std::string vpVideoReader::getExtension(const std::string &filename) void vpVideoReader::findLastFrameIndex() { if (!m_isOpen) { - vpERROR_TRACE("Use the open method before"); - throw(vpException(vpException::notInitialized, "file not yet opened")); + throw(vpException(vpException::notInitialized, "File not yet opened. Use the open() method before")); } if (m_imSequence != nullptr) { diff --git a/modules/io/src/video/vpVideoWriter.cpp b/modules/io/src/video/vpVideoWriter.cpp index f38d889ddd..e196b9f573 100644 --- a/modules/io/src/video/vpVideoWriter.cpp +++ b/modules/io/src/video/vpVideoWriter.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,7 +36,6 @@ \brief Write image sequences. */ -#include #include #include diff --git a/modules/sensor/src/framegrabber/1394/vp1394TwoGrabber.cpp b/modules/sensor/src/framegrabber/1394/vp1394TwoGrabber.cpp index ec30aa3f75..d80c64a1cc 100644 --- a/modules/sensor/src/framegrabber/1394/vp1394TwoGrabber.cpp +++ b/modules/sensor/src/framegrabber/1394/vp1394TwoGrabber.cpp @@ -40,6 +40,7 @@ #include #include +#include /* * Interface with libdc1394 2.x @@ -1279,22 +1280,22 @@ void vp1394TwoGrabber::setFormat7ROI(unsigned int left, unsigned int top, unsign setCapture(DC1394_ON); setTransmission(DC1394_ON); - } } - /*! +} +/*! - Open ohci and asign handle to it and get the camera nodes and - describe them as we find them. + Open ohci and asign handle to it and get the camera nodes and + describe them as we find them. - \param reset : If "true", reset the bus attached to the first - camera found. Bus reset may help to make firewire working if the - program was not properly stopped by a CTRL-C. + \param reset : If "true", reset the bus attached to the first + camera found. Bus reset may help to make firewire working if the + program was not properly stopped by a CTRL-C. - \exception initializationError : If a raw1394 handle can't be aquired, - or if no camera is found. + \exception initializationError : If a raw1394 handle can't be aquired, + or if no camera is found. - \sa close() - */ + \sa close() + */ void vp1394TwoGrabber::initialize(bool reset) { if (init == false) { @@ -1360,7 +1361,7 @@ void vp1394TwoGrabber::initialize(bool reset) "`ohci1394' are loaded \n" " - if you have read/write access to /dev/raw1394\n\n"); throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Unable to look for cameras")); - } + } #endif if (num_cameras == 0) { @@ -1398,7 +1399,7 @@ void vp1394TwoGrabber::initialize(bool reset) } init = true; -} + } } /*! @@ -1430,23 +1431,23 @@ void vp1394TwoGrabber::open() else { if (status == DC1394_ON) { vpTRACE("ISO transmission refuses to stop"); - } + } #ifdef VISP_HAVE_DC1394_FIND_CAMERAS // old API <= libdc1394-2.0.0-rc7 // No yet in the new API cameras[camera_id]->is_iso_on = status; #endif + } + //#ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API > + // libdc1394-2.0.0-rc7 } - //#ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API > - // libdc1394-2.0.0-rc7 + //#endif } - //#endif - } setCamera(camera_id); // setIsoSpeed(DC1394_ISO_SPEED_400); setCapture(DC1394_ON); setTransmission(DC1394_ON); camIsOpen[camera_id] = true; -} + } } /*! @@ -1496,8 +1497,8 @@ void vp1394TwoGrabber::close() #elif defined VISP_HAVE_DC1394_FIND_CAMERAS // old API <= libdc1394-2.0.0-rc7 dc1394_free_camera(cameras[i]); #endif - } } + } if (camIsOpen != nullptr) { delete[] camIsOpen; camIsOpen = nullptr; @@ -1538,21 +1539,21 @@ void vp1394TwoGrabber::close() } init = false; - } - } + } +} - /*! +/*! - Set the ring buffer size used for the capture. - To know the current ring buffer size see getRingBufferSize(). + Set the ring buffer size used for the capture. + To know the current ring buffer size see getRingBufferSize(). - \param size : Ring buffer size. + \param size : Ring buffer size. - \exception vpFrameGrabberException::settingError : If ring buffer size is - not valid. + \exception vpFrameGrabberException::settingError : If ring buffer size is + not valid. - \sa getRingBufferSize() - */ + \sa getRingBufferSize() +*/ void vp1394TwoGrabber::setRingBufferSize(unsigned int size) { if (size < 1) { @@ -2768,22 +2769,22 @@ void vp1394TwoGrabber::printCameraInfo() #elif defined VISP_HAVE_DC1394_FIND_CAMERAS // old API <= libdc1394-2.0.0-rc7 dc1394_print_feature_set(&features); #endif -} - std::cout << "----------------------------------------------------------" << std::endl; } + std::cout << "----------------------------------------------------------" << std::endl; +} - /*! +/*! - Converts the video mode identifier into a string containing the description - of the mode. + Converts the video mode identifier into a string containing the description + of the mode. - \param videomode : The camera capture video mode. + \param videomode : The camera capture video mode. - \return A string describing the mode, an empty string if the mode is not - supported. + \return A string describing the mode, an empty string if the mode is not + supported. - \sa string2videoMode() - */ + \sa string2videoMode() +*/ std::string vp1394TwoGrabber::videoMode2string(vp1394TwoVideoModeType videomode) { std::string _str = ""; diff --git a/modules/tracker/blob/include/visp3/blob/vpDot.h b/modules/tracker/blob/include/visp3/blob/vpDot.h index 5521783908..b432f5e5de 100644 --- a/modules/tracker/blob/include/visp3/blob/vpDot.h +++ b/modules/tracker/blob/include/visp3/blob/vpDot.h @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,16 +29,15 @@ * * Description: * Track a white dot. - * -*****************************************************************************/ + */ /*! \file vpDot.h \brief Track a white dot */ -#ifndef vpDot_hh -#define vpDot_hh +#ifndef VP_DOT_H +#define VP_DOT_H #include #include @@ -126,9 +124,6 @@ class VISP_EXPORT vpDot : public vpTracker #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS public: -#else -private: -#endif static const unsigned int SPIRAL_SEARCH_SIZE; /*!< Spiral size for the dot search. */ double m00; /*!< Considering the general distribution moments for \f$ N \f$ @@ -188,6 +183,7 @@ class VISP_EXPORT vpDot : public vpTracker \sa setComputeMoments() */ +#endif public: vpDot(); @@ -209,9 +205,12 @@ class VISP_EXPORT vpDot : public vpTracker inline vpColVector get_nij() const { vpColVector nij(3); - nij[0] = mu20 / m00; - nij[1] = mu11 / m00; - nij[2] = mu02 / m00; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + nij[index_0] = mu20 / m00; + nij[index_1] = mu11 / m00; + nij[index_2] = mu02 / m00; return nij; } @@ -232,16 +231,16 @@ class VISP_EXPORT vpDot : public vpTracker { vpRect bbox; - bbox.setRect(this->u_min, this->v_min, this->u_max - this->u_min + 1, this->v_max - this->v_min + 1); + bbox.setRect(m_u_min, m_v_min, (m_u_max - m_u_min) + 1, (m_v_max - m_v_min) + 1); - return (bbox); + return bbox; }; /*! * Return the location of the dot center of gravity. * * \return The coordinates of the center of gravity. */ - inline vpImagePoint getCog() const { return cog; } + inline vpImagePoint getCog() const { return m_cog; } /*! * Return the list of all the image points on the border of the dot. @@ -249,7 +248,7 @@ class VISP_EXPORT vpDot : public vpTracker * \warning Doesn't return the image points inside the dot anymore. To get * those points see getConnexities(). */ - inline std::list getEdges() const { return this->ip_edges_list; }; + inline std::list getEdges() const { return m_ip_edges_list; }; /*! * Return the list of all the image points inside the dot. @@ -257,41 +256,41 @@ class VISP_EXPORT vpDot : public vpTracker * \return The list of all the images points in the dot. * This list is updated after a call to track(). */ - inline std::list getConnexities() const { return this->ip_connexities_list; }; + inline std::list getConnexities() const { return m_ip_connexities_list; }; - inline double getGamma() const { return this->gamma; }; + inline double getGamma() const { return m_gamma; }; /*! * Return the precision of the gray level of the dot. It is a double * precision float witch value is in ]0,1]. 1 means full precision, whereas * values close to 0 show a very bad precision. */ - double getGrayLevelPrecision() const { return grayLevelPrecision; } - double getMaxDotSize() const { return this->maxDotSizePercentage; } + double getGrayLevelPrecision() const { return m_grayLevelPrecision; } + double getMaxDotSize() const { return m_maxDotSizePercentage; } /*! * Return the mean gray level value of the dot. */ - double getMeanGrayLevel() const { return (this->mean_gray_level); }; + double getMeanGrayLevel() const { return m_mean_gray_level; }; /*! * \return a vpPolygon made from the edges of the dot. */ - vpPolygon getPolygon() const { return (vpPolygon(ip_edges_list)); }; + vpPolygon getPolygon() const { return (vpPolygon(m_ip_edges_list)); }; /*! * Return the width of the dot. * * \sa getHeight() */ - inline unsigned int getWidth() const { return (this->u_max - this->u_min + 1); }; + inline unsigned int getWidth() const { return ((m_u_max - m_u_min) + 1); }; /*! * Return the width of the dot. * * \sa getHeight() */ - inline unsigned int getHeight() const { return (this->v_max - this->v_min + 1); }; + inline unsigned int getHeight() const { return ((m_v_max - m_v_min) + 1); }; void initTracking(const vpImage &I); void initTracking(const vpImage &I, const vpImagePoint &ip); @@ -306,9 +305,9 @@ class VISP_EXPORT vpDot : public vpTracker void print(std::ostream &os) { os << *this << std::endl; } /*! - * Initialize the dot coordinates with \e ip. + * Initialize the dot center of gravity coordinates with \e cog. */ - inline void setCog(const vpImagePoint &ip) { this->cog = ip; } + inline void setCog(const vpImagePoint &cog) { m_cog = cog; } /*! * Activates the dot's moments computation. @@ -323,15 +322,15 @@ class VISP_EXPORT vpDot : public vpTracker * The coordinates of the region's centroid (u, v) can be computed from the * moments by \f$u=\frac{m10}{m00}\f$ and \f$v=\frac{m01}{m00}\f$. */ - void setComputeMoments(bool activate) { compute_moment = activate; } + void setComputeMoments(bool activate) { m_compute_moment = activate; } /*! * Set the type of connexity: 4 or 8. */ - void setConnexity(vpConnexityType type) { this->connexityType = type; }; + void setConnexity(const vpConnexityType &connexityType) { m_connexityType = connexityType; }; void setMaxDotSize(double percentage); - void setGrayLevelMin(const unsigned int &level_min) { this->gray_level_min = level_min; }; - void setGrayLevelMax(const unsigned int &level_max) { this->gray_level_max = level_max; }; + void setGrayLevelMin(const unsigned int &level_min) { m_gray_level_min = level_min; }; + void setGrayLevelMax(const unsigned int &level_max) { m_gray_level_max = level_max; }; void setGrayLevelPrecision(const double &grayLevelPrecision); /*! @@ -348,72 +347,135 @@ class VISP_EXPORT vpDot : public vpTracker * \sa setGraphicsThickness() */ - void setGraphics(bool activate) { graphics = activate; } + void setGraphics(bool activate) { m_graphics = activate; } /*! * Modify the default thickness that is set to 1 of the drawings in overlay * when setGraphics() is enabled. * * \sa setGraphics() */ - void setGraphicsThickness(unsigned int t) { this->thickness = t; }; + void setGraphicsThickness(unsigned int thickness) { m_thickness = thickness; }; void track(const vpImage &I); void track(const vpImage &I, vpImagePoint &ip); + // Static Functions +public: + static void display(const vpImage &I, const vpImagePoint &cog, + const std::list &edges_list, vpColor color = vpColor::red, + unsigned int thickness = 1); + static void display(const vpImage &I, const vpImagePoint &cog, const std::list &edges_list, + vpColor color = vpColor::red, unsigned int thickness = 1); + +#ifndef VISP_BUILD_DEPRECATED_FUNCTIONS +private: + static const unsigned int SPIRAL_SEARCH_SIZE; /*!< Spiral size for the dot search. */ + + double m00; /*!< Considering the general distribution moments for \f$ N \f$ + points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N} + u_h^i v_h^j \f$, \f$ m_{00} \f$ is a zero order moment obtained + with \f$i = j = 0 \f$. + + \sa setComputeMoments() + */ + double m01; /*!< Considering the general distribution moments for \f$ N \f$ + points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N} + u_h^i v_h^j \f$, \f$ m_{01} \f$ is a first order moment + obtained with \f$i = 0 \f$ and \f$ j = 1 \f$. + + \sa setComputeMoments() + */ + double m10; /*!< Considering the general distribution moments for \f$ N \f$ + points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N} + u_h^i v_h^j \f$, \f$ m_{10} \f$ is a first order moment + obtained with \f$i = 1 \f$ and \f$ j = 0 \f$. + + \sa setComputeMoments() + */ + double m11; /*!< Considering the general distribution moments for \f$ N \f$ + points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N} + u_h^i v_h^j \f$, \f$ m_{11} \f$ is a first order moment + obtained with \f$i = 1 \f$ and \f$ j = 1 \f$. + + \sa setComputeMoments() + */ + double m20; /*!< Considering the general distribution moments for \f$ N \f$ + points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N} + u_h^i v_h^j \f$, \f$ m_{20} \f$ is a second order moment + obtained with \f$i = 2 \f$ and \f$ j = 0 \f$. + + \sa setComputeMoments() + */ + double m02; /*!< Considering the general distribution moments for \f$ N \f$ + points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N} + u_h^i v_h^j \f$, \f$ m_{02} \f$ is a second order moment + obtained with \f$i = 0 \f$ and \f$ j = 2 \f$. + + \sa setComputeMoments() + */ + double mu11; /*!< \f$ \mu_{11} \f$ is a second order centered moment defined + by: \f$ \mu_{11} = m_{11} - \frac{m_{10}}{m_{00}}m_{01} \f$ + + \sa setComputeMoments() + */ + double mu20; /*!< \f$ \mu_{20} \f$ is a second order centered moment defined + by: \f$ \mu_{20} = m_{20} - \frac{m_{10}}{m_{00}}m_{10} \f$ + + \sa setComputeMoments() + */ + double mu02; /*!< \f$ \mu_{02} \f$ is a second order centered moment defined + by: \f$ \mu_{02} = m_{02} - \frac{m_{01}}{m_{00}}m_{01} \f$ + + \sa setComputeMoments() + */ +#endif + private: //! internal use only - std::list ip_connexities_list; + std::list m_ip_connexities_list; //! List of border points - std::list ip_edges_list; + std::list m_ip_edges_list; /*! Type of connexity \warning In previous version this variable was called connexity */ - vpConnexityType connexityType; + vpConnexityType m_connexityType; - //! coordinates of the point center of gravity - vpImagePoint cog; + //! Coordinates of the point center of gravity + vpImagePoint m_cog; // Bounding box - unsigned int u_min, u_max, v_min, v_max; + unsigned int m_u_min, m_u_max, m_v_min, m_v_max; // Flag used to allow display - bool graphics; + bool m_graphics; - unsigned int thickness; // Graphics thickness + unsigned int m_thickness; // Graphics thickness - double maxDotSizePercentage; - unsigned char gray_level_out; + double m_maxDotSizePercentage; + unsigned char m_gray_level_out; - double mean_gray_level; // Mean gray level of the dot - unsigned int gray_level_min; // left threshold for binarization - unsigned int gray_level_max; // right threshold for binarization - double grayLevelPrecision; // precision of the gray level of the dot. + double m_mean_gray_level; // Mean gray level of the dot + unsigned int m_gray_level_min; // left threshold for binarization + unsigned int m_gray_level_max; // right threshold for binarization + double m_grayLevelPrecision; // precision of the gray level of the dot. // It is a double precision float witch value is in ]0,1]. // 1 means full precision, whereas values close to 0 show a very bad // precision - double gamma; + double m_gamma; //! flag : true moment are computed - bool compute_moment; - double nbMaxPoint; + bool m_compute_moment; + double m_nbMaxPoint; void init(); void setGrayLevelOut(); - bool connexe(const vpImage &I, unsigned int u, unsigned int v, double &mean_value, double &u_cog, - double &v_cog, double &n); - bool connexe(const vpImage &I, unsigned int u, unsigned int v, double &mean_value, double &u_cog, - double &v_cog, double &n, std::vector &checkTab); + bool connexe(const vpImage &I, unsigned int u, unsigned int v, double &mean_value, + vpImagePoint &uv_cog, unsigned int &npoints); + bool connexe(const vpImage &I, unsigned int u, unsigned int v, double &mean_value, + vpImagePoint &uv_cog, unsigned int &npoints, std::vector &checkTab); void COG(const vpImage &I, double &u, double &v); - - // Static Functions -public: - static void display(const vpImage &I, const vpImagePoint &cog, - const std::list &edges_list, vpColor color = vpColor::red, - unsigned int thickness = 1); - static void display(const vpImage &I, const vpImagePoint &cog, const std::list &edges_list, - vpColor color = vpColor::red, unsigned int thickness = 1); }; END_VISP_NAMESPACE diff --git a/modules/tracker/blob/include/visp3/blob/vpDot2.h b/modules/tracker/blob/include/visp3/blob/vpDot2.h index 24bb21bec7..2916d542af 100644 --- a/modules/tracker/blob/include/visp3/blob/vpDot2.h +++ b/modules/tracker/blob/include/visp3/blob/vpDot2.h @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,16 +29,15 @@ * * Description: * Track a white dot. - * -*****************************************************************************/ + */ -/* +/*! \file vpDot2.h \brief This tracker is meant to track some zones on a vpImage. */ -#ifndef vpDot2_hh -#define vpDot2_hh +#ifndef VP_DOT2_H +#define VP_DOT2_H #include #include @@ -147,9 +145,13 @@ class VISP_EXPORT vpDot2 : public vpTracker inline vpColVector get_nij() const { vpColVector nij(3); - nij[0] = mu20 / m00; - nij[1] = mu11 / m00; - nij[2] = mu02 / m00; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + + nij[index_0] = mu20 / m00; + nij[index_1] = mu11 / m00; + nij[index_2] = mu02 / m00; return nij; } @@ -165,8 +167,8 @@ class VISP_EXPORT vpDot2 : public vpTracker { vpRect bbox; - bbox.setRect(this->bbox_u_min, this->bbox_v_min, (this->bbox_u_max - this->bbox_u_min) + 1, - (this->bbox_v_max - this->bbox_v_min) + 1); + bbox.setRect(m_bbox_u_min, m_bbox_v_min, (m_bbox_u_max - m_bbox_u_min) + 1, + (m_bbox_v_max - m_bbox_v_min) + 1); return bbox; }; @@ -176,7 +178,7 @@ class VISP_EXPORT vpDot2 : public vpTracker * * \return The coordinates of the center of gravity. */ - inline vpImagePoint getCog() const { return cog; } + inline vpImagePoint getCog() const { return m_cog; } double getDistance(const vpDot2 &distantDot) const; /*! @@ -186,7 +188,7 @@ class VISP_EXPORT vpDot2 : public vpTracker * \param edges_list : The list of all the images points on the dot * border. This list is update after a call to track(). */ - void getEdges(std::list &edges_list) const { edges_list = this->ip_edges_list; }; + void getEdges(std::list &edges_list) const { edges_list = m_ip_edges_list; }; /*! * Return the list of all the image points on the dot @@ -195,7 +197,7 @@ class VISP_EXPORT vpDot2 : public vpTracker * \return The list of all the images points on the dot * border. This list is update after a call to track(). */ - std::list getEdges() const { return this->ip_edges_list; }; + std::list getEdges() const { return m_ip_edges_list; }; /*! * Get the percentage of sampled points that are considered non conform @@ -203,24 +205,24 @@ class VISP_EXPORT vpDot2 : public vpTracker * * \sa setEllipsoidBadPointsPercentage() */ - double getEllipsoidBadPointsPercentage() const { return allowedBadPointsPercentage_; } + double getEllipsoidBadPointsPercentage() const { return m_allowedBadPointsPercentage; } double getEllipsoidShapePrecision() const; void getFreemanChain(std::list &freeman_chain) const; - inline double getGamma() const { return this->gamma; }; + inline double getGamma() const { return m_gamma; }; /*! * Return the color level of pixels inside the dot. * * \sa getGrayLevelMax() */ - inline unsigned int getGrayLevelMin() const { return gray_level_min; }; + inline unsigned int getGrayLevelMin() const { return m_gray_level_min; }; /*! * Return the color level of pixels inside the dot. * * \sa getGrayLevelMin() */ - inline unsigned int getGrayLevelMax() const { return gray_level_max; }; + inline unsigned int getGrayLevelMax() const { return m_gray_level_max; }; double getGrayLevelPrecision() const; double getHeight() const; @@ -229,12 +231,12 @@ class VISP_EXPORT vpDot2 : public vpTracker /*! * \return The mean gray level value of the dot. */ - double getMeanGrayLevel() const { return (this->mean_gray_level); }; + double getMeanGrayLevel() const { return m_mean_gray_level; }; /*! * \return a vpPolygon made from the edges of the dot. */ - vpPolygon getPolygon() const { return (vpPolygon(ip_edges_list)); }; + vpPolygon getPolygon() const { return (vpPolygon(m_ip_edges_list)); }; double getSizePrecision() const; double getWidth() const; @@ -256,7 +258,7 @@ class VISP_EXPORT vpDot2 : public vpTracker /*! * Initialize the dot coordinates with \e ip. */ - inline void setCog(const vpImagePoint &ip) { this->cog = ip; } + inline void setCog(const vpImagePoint &ip) { m_cog = ip; } /*! * Activates the dot's moments computation. @@ -271,7 +273,7 @@ class VISP_EXPORT vpDot2 : public vpTracker * The coordinates of the region's centroid (u, v) can be computed from the * moments by \f$u=\frac{m10}{m00}\f$ and \f$v=\frac{m01}{m00}\f$. */ - void setComputeMoments(bool activate) { compute_moment = activate; } + void setComputeMoments(bool activate) { m_compute_moment = activate; } /*! * Set the percentage of sampled points that are considered non conform @@ -288,13 +290,13 @@ class VISP_EXPORT vpDot2 : public vpTracker void setEllipsoidBadPointsPercentage(const double &percentage = 0.0) { if (percentage < 0.) { - allowedBadPointsPercentage_ = 0.; + m_allowedBadPointsPercentage = 0.; } else if (percentage > 1.) { - allowedBadPointsPercentage_ = 1.; + m_allowedBadPointsPercentage = 1.; } else { - allowedBadPointsPercentage_ = percentage; + m_allowedBadPointsPercentage = percentage; } } @@ -313,7 +315,7 @@ class VISP_EXPORT vpDot2 : public vpTracker * * \sa setGraphicsThickness() */ - void setGraphics(bool activate) { graphics = activate; } + void setGraphics(bool activate) { m_graphics = activate; } /*! * Modify the default thickness that is set to 1 of the drawings in overlay @@ -321,7 +323,7 @@ class VISP_EXPORT vpDot2 : public vpTracker * * \sa setGraphics() */ - void setGraphicsThickness(unsigned int t) { this->thickness = t; }; + void setGraphicsThickness(unsigned int thickness) { m_thickness = thickness; }; /*! * Set the color level of the dot to search a dot in a region of interest. This @@ -335,11 +337,12 @@ class VISP_EXPORT vpDot2 : public vpTracker */ inline void setGrayLevelMin(const unsigned int &min) { - if (min > 255) { - this->gray_level_min = 255; + const unsigned int val_max = 255; + if (min > val_max) { + m_gray_level_min = val_max; } else { - this->gray_level_min = min; + m_gray_level_min = min; } }; @@ -353,11 +356,12 @@ class VISP_EXPORT vpDot2 : public vpTracker */ inline void setGrayLevelMax(const unsigned int &max) { - if (max > 255) { - this->gray_level_max = 255; + const unsigned int val_max = 255; + if (max > val_max) { + m_gray_level_max = val_max; } else { - this->gray_level_max = max; + m_gray_level_max = max; } }; @@ -472,7 +476,7 @@ class VISP_EXPORT vpDot2 : public vpTracker * \sa getFirstBorder_v() */ - unsigned int getFirstBorder_u() const { return this->firstBorder_u; } + unsigned int getFirstBorder_u() const { return m_firstBorder_u; } /*! Get the starting point on a dot border. The dot border is @@ -480,7 +484,7 @@ class VISP_EXPORT vpDot2 : public vpTracker * \sa getFirstBorder_u() */ - unsigned int getFirstBorder_v() const { return this->firstBorder_v; } + unsigned int getFirstBorder_v() const { return m_firstBorder_v; } bool computeFreemanChainElement(const vpImage &I, const unsigned int &u, const unsigned int &v, unsigned int &element); @@ -499,41 +503,66 @@ class VISP_EXPORT vpDot2 : public vpTracker void setArea(const vpRect &a); unsigned char getMeanGrayLevel(vpImage &I) const; - //! coordinates (float) of the point center of gravity - vpImagePoint cog; - - double width; - double height; - double surface; - unsigned int gray_level_min; // minumum gray level for the dot. Pixel with lower level don't belong to this dot. - - unsigned int gray_level_max; // maximum gray level for the dot. Pixel with higher level don't belong to this dot. - double mean_gray_level; // Mean gray level of the dot - double grayLevelPrecision; - double gamma; - double sizePrecision; - double ellipsoidShapePrecision; - double maxSizeSearchDistancePrecision; - double allowedBadPointsPercentage_; + + typedef struct vpSearchDotsInAreaGoodGermData + { + const vpImage &m_I; + const vpRect &m_area; + unsigned int &m_u; + unsigned int &m_v; + std::list &m_niceDots; + std::list &m_badDotsVector; + + vpSearchDotsInAreaGoodGermData(const vpImage &I, const vpRect &area, + unsigned int &u, unsigned int &v, + std::list &niceDots, std::list &badDotsVector) + : m_I(I) + , m_area(area) + , m_u(u) + , m_v(v) + , m_niceDots(niceDots) + , m_badDotsVector(badDotsVector) + { + + } + } vpSearchDotsInAreaGoodGermData; + + void searchDotsAreaGoodGerm(vpSearchDotsInAreaGoodGermData &data); + //! Coordinates (float) of the point center of gravity + vpImagePoint m_cog; + + double m_width; + double m_height; + double m_surface; + unsigned int m_gray_level_min; // minumum gray level for the dot. Pixel with lower level don't belong to this dot. + + unsigned int m_gray_level_max; // maximum gray level for the dot. Pixel with higher level don't belong to this dot. + double m_mean_gray_level; // Mean gray level of the dot + double m_grayLevelPrecision; + double m_gamma; + double m_sizePrecision; + double m_ellipsoidShapePrecision; + double m_maxSizeSearchDistPrecision; + double m_allowedBadPointsPercentage; // Area where the dot is to search - vpRect area; + vpRect m_area; // other - std::list direction_list; - std::list ip_edges_list; + std::list m_direction_list; + std::list m_ip_edges_list; // flag - bool compute_moment; // true moment are computed - bool graphics; // true for graphic overlay display + bool m_compute_moment; // true moment are computed + bool m_graphics; // true for graphic overlay display - unsigned int thickness; // Graphics thickness + unsigned int m_thickness; // Graphics thickness // Bounding box - int bbox_u_min, bbox_u_max, bbox_v_min, bbox_v_max; + int m_bbox_u_min, m_bbox_u_max, m_bbox_v_min, m_bbox_v_max; // The first point coordinate on the dot border - unsigned int firstBorder_u; - unsigned int firstBorder_v; + unsigned int m_firstBorder_u; + unsigned int m_firstBorder_v; }; diff --git a/modules/tracker/blob/src/dots/vpDot.cpp b/modules/tracker/blob/src/dots/vpDot.cpp index e641088202..3504d57ac3 100644 --- a/modules/tracker/blob/src/dots/vpDot.cpp +++ b/modules/tracker/blob/src/dots/vpDot.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,7 @@ * * Description: * Track a white dot. - * - * Authors: - * Aurelien Yol - * -*****************************************************************************/ + */ /* \file vpDot.cpp @@ -59,26 +54,27 @@ BEGIN_VISP_NAMESPACE const unsigned int vpDot::SPIRAL_SEARCH_SIZE = 350; /*! - Initialize the tracker with default parameters. - - connexityType is set to 4 (see setConnexityType()) + - connexity type is set to 4 (see setConnexityType()) - dot maximal size is set to 25% of the image size (see setMaxDotSize()) */ void vpDot::init() { - cog.set_u(0); - cog.set_v(0); - - compute_moment = false; - graphics = false; - thickness = 1; - maxDotSizePercentage = 0.25; // 25 % of the image size - - mean_gray_level = 0; - gray_level_min = 128; - gray_level_max = 255; - grayLevelPrecision = 0.85; - gamma = 1.5; + const unsigned int val_max = 255; + const unsigned int val_median = 128; + m_cog.set_u(0); + m_cog.set_v(0); + + m_compute_moment = false; + m_graphics = false; + m_thickness = 1; + m_maxDotSizePercentage = 0.25; // 25 % of the image size + + m_mean_gray_level = 0; + m_gray_level_min = val_median; + m_gray_level_max = val_max; + m_grayLevelPrecision = 0.85; + m_gamma = 1.5; m00 = 0; m11 = 0; @@ -90,79 +86,90 @@ void vpDot::init() mu02 = 0; mu20 = 0; - connexityType = CONNEXITY_4; + m_connexityType = CONNEXITY_4; - u_min = 0; - u_max = 0; - v_min = 0; - v_max = 0; + m_u_min = 0; + m_u_max = 0; + m_v_min = 0; + m_v_max = 0; - gray_level_out = 0; - nbMaxPoint = 0; + m_gray_level_out = 0; + m_nbMaxPoint = 0; } vpDot::vpDot() - : m00(0.), m01(0.), m10(0.), m11(0.), m20(0.), m02(0.), mu11(0.), mu20(0.), mu02(0.), ip_connexities_list(), - ip_edges_list(), connexityType(CONNEXITY_4), cog(), u_min(0), u_max(0), v_min(0), v_max(0), graphics(false), - thickness(1), maxDotSizePercentage(0.25), gray_level_out(0), mean_gray_level(0), gray_level_min(128), - gray_level_max(255), grayLevelPrecision(0.85), gamma(1.5), compute_moment(false), nbMaxPoint(0) -{ } + : m00(0.), m01(0.), m10(0.), m11(0.), m20(0.), m02(0.), mu11(0.), mu20(0.), mu02(0.), m_ip_connexities_list(), + m_ip_edges_list(), m_connexityType(CONNEXITY_4), m_cog(), m_u_min(0), m_u_max(0), m_v_min(0), m_v_max(0), m_graphics(false), + m_thickness(1), m_maxDotSizePercentage(0.25), m_gray_level_out(0), m_mean_gray_level(0), + m_grayLevelPrecision(0.85), m_gamma(1.5), m_compute_moment(false), m_nbMaxPoint(0) +{ + const unsigned int val_min = 128; + const unsigned int val_max = 255; + m_gray_level_min = val_min; + m_gray_level_max = val_max; +} /*! \brief Constructor with initialization of the dot location. - \param ip : An image point with sub-pixel coordinates. + \param cog : An image point with sub-pixel coordinates corresponding to the blob center of gravity. */ -vpDot::vpDot(const vpImagePoint &ip) - : m00(0.), m01(0.), m10(0.), m11(0.), m20(0.), m02(0.), mu11(0.), mu20(0.), mu02(0.), ip_connexities_list(), - ip_edges_list(), connexityType(CONNEXITY_4), cog(ip), u_min(0), u_max(0), v_min(0), v_max(0), graphics(false), - thickness(1), maxDotSizePercentage(0.25), gray_level_out(0), mean_gray_level(0), gray_level_min(128), - gray_level_max(255), grayLevelPrecision(0.85), gamma(1.5), compute_moment(false), nbMaxPoint(0) -{ } +vpDot::vpDot(const vpImagePoint &cog) + : m00(0.), m01(0.), m10(0.), m11(0.), m20(0.), m02(0.), mu11(0.), mu20(0.), mu02(0.), m_ip_connexities_list(), + m_ip_edges_list(), m_connexityType(CONNEXITY_4), m_cog(cog), m_u_min(0), m_u_max(0), m_v_min(0), m_v_max(0), m_graphics(false), + m_thickness(1), m_maxDotSizePercentage(0.25), m_gray_level_out(0), m_mean_gray_level(0), + m_grayLevelPrecision(0.85), m_gamma(1.5), m_compute_moment(false), m_nbMaxPoint(0) +{ + const unsigned int val_min = 128; + const unsigned int val_max = 255; + m_gray_level_min = val_min; + m_gray_level_max = val_max; +} /*! \brief Copy constructor. */ vpDot::vpDot(const vpDot &d) : vpTracker(d), m00(0.), m01(0.), m10(0.), m11(0.), m20(0.), m02(0.), mu11(0.), mu20(0.), mu02(0.), - ip_connexities_list(), ip_edges_list(), connexityType(CONNEXITY_4), cog(), u_min(0), u_max(0), v_min(0), v_max(0), - graphics(false), thickness(1), maxDotSizePercentage(0.25), gray_level_out(0), mean_gray_level(0), - gray_level_min(128), gray_level_max(255), grayLevelPrecision(0.85), gamma(1.5), compute_moment(false), nbMaxPoint(0) + m_ip_connexities_list(), m_ip_edges_list(), m_connexityType(CONNEXITY_4), m_cog(), m_u_min(0), m_u_max(0), m_v_min(0), m_v_max(0), + m_graphics(false), m_thickness(1), m_maxDotSizePercentage(0.25), m_gray_level_out(0), m_mean_gray_level(0), + m_grayLevelPrecision(0.85), m_gamma(1.5), m_compute_moment(false), m_nbMaxPoint(0) { + *this = d; } /*! \brief Destructor. */ -vpDot::~vpDot() { ip_connexities_list.clear(); } +vpDot::~vpDot() { m_ip_connexities_list.clear(); } /*! \brief Copy operator. */ vpDot &vpDot::operator=(const vpDot &d) { - ip_edges_list = d.ip_edges_list; - ip_connexities_list = d.ip_connexities_list; - connexityType = d.connexityType; - cog = d.getCog(); - - u_min = d.u_min; - v_min = d.v_min; - u_max = d.u_max; - v_max = d.v_max; - - graphics = d.graphics; - thickness = d.thickness; - maxDotSizePercentage = d.maxDotSizePercentage; - gray_level_out = d.gray_level_out; - mean_gray_level = d.mean_gray_level; - gray_level_min = d.gray_level_min; - gray_level_max = d.gray_level_max; - grayLevelPrecision = d.grayLevelPrecision; - gamma = d.gamma; - compute_moment = d.compute_moment; - nbMaxPoint = d.nbMaxPoint; + m_ip_edges_list = d.m_ip_edges_list; + m_ip_connexities_list = d.m_ip_connexities_list; + m_connexityType = d.m_connexityType; + m_cog = d.getCog(); + + m_u_min = d.m_u_min; + m_v_min = d.m_v_min; + m_u_max = d.m_u_max; + m_v_max = d.m_v_max; + + m_graphics = d.m_graphics; + m_thickness = d.m_thickness; + m_maxDotSizePercentage = d.m_maxDotSizePercentage; + m_gray_level_out = d.m_gray_level_out; + m_mean_gray_level = d.m_mean_gray_level; + m_gray_level_min = d.m_gray_level_min; + m_gray_level_max = d.m_gray_level_max; + m_grayLevelPrecision = d.m_grayLevelPrecision; + m_gamma = d.m_gamma; + m_compute_moment = d.m_compute_moment; + m_nbMaxPoint = d.m_nbMaxPoint; m00 = d.m00; m01 = d.m01; @@ -178,9 +185,9 @@ vpDot &vpDot::operator=(const vpDot &d) return *this; } -bool vpDot::operator!=(const vpDot &d) const { return (cog != d.getCog()); } +bool vpDot::operator!=(const vpDot &d) const { return (m_cog != d.getCog()); } -bool vpDot::operator==(const vpDot &d) const { return (cog == d.getCog()); } +bool vpDot::operator==(const vpDot &d) const { return (m_cog == d.getCog()); } /*! @@ -195,13 +202,13 @@ bool vpDot::operator==(const vpDot &d) const { return (cog == d.getCog()); } */ void vpDot::setGrayLevelOut() { - if (gray_level_min == 0) { - if (gray_level_max == 255) { - // gray_level_min = 0 and gray_level_max = 255: this should not occur - // vpERROR_TRACE("Unable to choose a good \"out\" level") ; + if (m_gray_level_min == 0) { + const unsigned int val_max = 255; + if (m_gray_level_max == val_max) { + // m_gray_level_min = 0 and m_gray_level_max = 255: this should not occur throw(vpTrackingException(vpTrackingException::initializationError, "Unable to choose a good \"out\" level")); } - gray_level_out = static_cast(gray_level_max + 1u); + m_gray_level_out = static_cast(m_gray_level_max + 1u); } } @@ -214,20 +221,20 @@ void vpDot::setGrayLevelOut() \warning The content of the image is modified thanks to setGrayLevelOut() called before. This method choose a gray level (default is 0) used to modify the "in" dot level in "out" dot - level. This gray level is here needed to stop the recursivity . The + level. This gray level is here needed to stop the recursivity. The pixels of the dot are set to this new gray level "\out\". \return vpDot::out if an error occurs, vpDot::in otherwise. \sa setGrayLevelOut() */ - -bool vpDot::connexe(const vpImage &I, unsigned int u, unsigned int v, double &mean_value, double &u_cog, - double &v_cog, double &n) +bool vpDot::connexe(const vpImage &I, unsigned int u, unsigned int v, double &mean_value, + vpImagePoint &uv_cog, unsigned int &npoints) { std::vector checkTab(I.getWidth() * I.getHeight(), false); - return connexe(I, u, v, mean_value, u_cog, v_cog, n, checkTab); + return connexe(I, u, v, mean_value, uv_cog, npoints, checkTab); } + /*! Perform the tracking of a dot by connex components. @@ -244,9 +251,8 @@ bool vpDot::connexe(const vpImage &I, unsigned int u, unsigned in \sa setGrayLevelOut() */ - -bool vpDot::connexe(const vpImage &I, unsigned int u, unsigned int v, double &mean_value, double &u_cog, - double &v_cog, double &n, std::vector &checkTab) +bool vpDot::connexe(const vpImage &I, unsigned int u, unsigned int v, double &mean_value, + vpImagePoint &uv_cog, unsigned int &npoints, std::vector &checkTab) { unsigned int width = I.getWidth(); @@ -265,40 +271,40 @@ bool vpDot::connexe(const vpImage &I, unsigned int u, unsigned in ip.set_u(u); ip.set_v(v); - if ((I[v][u] >= gray_level_min) && (I[v][u] <= gray_level_max)) { + if ((I[v][u] >= m_gray_level_min) && (I[v][u] <= m_gray_level_max)) { checkTab[(v * I.getWidth()) + u] = true; - ip_connexities_list.push_back(ip); + m_ip_connexities_list.push_back(ip); - u_cog += u; - v_cog += v; - n += 1; + uv_cog.set_u(uv_cog.get_u() + u); + uv_cog.set_v(uv_cog.get_v() + v); + ++npoints; - if (n > nbMaxPoint) { + if (npoints > m_nbMaxPoint) { throw(vpTrackingException(vpTrackingException::featureLostError, - "Too many point %lf (%lf%% of image size). " + "Too many point %u (%f%% of image size). " "This threshold can be modified using the setMaxDotSize() " "method.", - n, n / (I.getWidth() * I.getHeight()), nbMaxPoint, maxDotSizePercentage)); + npoints, static_cast(npoints) / (I.getWidth() * I.getHeight()), m_nbMaxPoint, m_maxDotSizePercentage)); } // Bounding box update - if (u < this->u_min) { - this->u_min = u; + if (u < m_u_min) { + m_u_min = u; } - if (u > this->u_max) { - this->u_max = u; + if (u > m_u_max) { + m_u_max = u; } - if (v < this->v_min) { - this->v_min = v; + if (v < m_v_min) { + m_v_min = v; } - if (v > this->v_max) { - this->v_max = v; + if (v > m_v_max) { + m_v_max = v; } // Mean value of the dot intensities - mean_value = ((mean_value * (n - 1)) + I[v][u]) / n; - if (compute_moment == true) { + mean_value = ((mean_value * (npoints - 1)) + I[v][u]) / npoints; + if (m_compute_moment == true) { ++m00; m10 += u; m01 += v; @@ -315,7 +321,7 @@ bool vpDot::connexe(const vpImage &I, unsigned int u, unsigned in if (u >= 1) { if (!checkTab[(u - 1) + (v * I.getWidth())]) { - if (!connexe(I, u - 1, v, mean_value, u_cog, v_cog, n, checkTab)) { + if (!connexe(I, u - 1, v, mean_value, uv_cog, npoints, checkTab)) { edge = true; } } @@ -323,7 +329,7 @@ bool vpDot::connexe(const vpImage &I, unsigned int u, unsigned in if ((u + 1) < I.getWidth()) { if (!checkTab[u + 1 + (v * I.getWidth())]) { - if (!connexe(I, u + 1, v, mean_value, u_cog, v_cog, n, checkTab)) { + if (!connexe(I, u + 1, v, mean_value, uv_cog, npoints, checkTab)) { edge = true; } } @@ -331,7 +337,7 @@ bool vpDot::connexe(const vpImage &I, unsigned int u, unsigned in if (v >= 1) { if (!checkTab[u + ((v - 1) * I.getWidth())]) { - if (!connexe(I, u, v - 1, mean_value, u_cog, v_cog, n, checkTab)) { + if (!connexe(I, u, v - 1, mean_value, uv_cog, npoints, checkTab)) { edge = true; } } @@ -339,16 +345,16 @@ bool vpDot::connexe(const vpImage &I, unsigned int u, unsigned in if ((v + 1) < I.getHeight()) { if (!checkTab[u + ((v + 1) * I.getWidth())]) { - if (!connexe(I, u, v + 1, mean_value, u_cog, v_cog, n, checkTab)) { + if (!connexe(I, u, v + 1, mean_value, uv_cog, npoints, checkTab)) { edge = true; } } } - if (connexityType == CONNEXITY_8) { + if (m_connexityType == CONNEXITY_8) { if ((v >= 1) && (u >= 1)) { if (!checkTab[(u - 1) + ((v - 1) * I.getWidth())]) { - if (!connexe(I, u - 1, v - 1, mean_value, u_cog, v_cog, n, checkTab)) { + if (!connexe(I, u - 1, v - 1, mean_value, uv_cog, npoints, checkTab)) { edge = true; } } @@ -356,7 +362,7 @@ bool vpDot::connexe(const vpImage &I, unsigned int u, unsigned in if ((v >= 1) && ((u + 1) < I.getWidth())) { if (!checkTab[u + 1 + ((v - 1) * I.getWidth())]) { - if (!connexe(I, u + 1, v - 1, mean_value, u_cog, v_cog, n, checkTab)) { + if (!connexe(I, u + 1, v - 1, mean_value, uv_cog, npoints, checkTab)) { edge = true; } } @@ -364,7 +370,7 @@ bool vpDot::connexe(const vpImage &I, unsigned int u, unsigned in if (((v + 1) < I.getHeight()) && (u >= 1)) { if (!checkTab[(u - 1) + ((v + 1) * I.getWidth())]) { - if (!connexe(I, u - 1, v + 1, mean_value, u_cog, v_cog, n, checkTab)) { + if (!connexe(I, u - 1, v + 1, mean_value, uv_cog, npoints, checkTab)) { edge = true; } } @@ -372,7 +378,7 @@ bool vpDot::connexe(const vpImage &I, unsigned int u, unsigned in if (((v + 1) < I.getHeight()) && ((u + 1) < I.getWidth())) { if (!checkTab[u + 1 + ((v + 1) * I.getWidth())]) { - if (!connexe(I, u + 1, v + 1, mean_value, u_cog, v_cog, n, checkTab)) { + if (!connexe(I, u + 1, v + 1, mean_value, uv_cog, npoints, checkTab)) { edge = true; } } @@ -380,10 +386,10 @@ bool vpDot::connexe(const vpImage &I, unsigned int u, unsigned in } if (edge) { - ip_edges_list.push_back(ip); - if (graphics == true) { + m_ip_edges_list.push_back(ip); + if (m_graphics == true) { vpImagePoint ip_(ip); - for (unsigned int t = 0; t < thickness; ++t) { + for (unsigned int t = 0; t < m_thickness; ++t) { ip_.set_u(ip.get_u() + t); vpDisplay::displayPoint(I, ip_, vpColor::red); } @@ -417,11 +423,11 @@ void vpDot::COG(const vpImage &I, double &u, double &v) { // Set the maximal number of points considering the maximal dot size // image percentage - nbMaxPoint = (I.getWidth() * I.getHeight()) * maxDotSizePercentage; + m_nbMaxPoint = (I.getWidth() * I.getHeight()) * m_maxDotSizePercentage; // segmentation de l'image apres seuillage // (etiquetage des composante connexe) - if (compute_moment) { + if (m_compute_moment) { m00 = 0; m11 = 0; m02 = 0; @@ -433,51 +439,21 @@ void vpDot::COG(const vpImage &I, double &u, double &v) mu02 = 0; } - double u_cog = 0; - double v_cog = 0; - double npoint = 0; - this->mean_gray_level = 0; + vpImagePoint uv_cog(0, 0); + unsigned int npoints = 0; + m_mean_gray_level = 0; - ip_connexities_list.clear(); - ip_edges_list.clear(); + m_ip_connexities_list.clear(); + m_ip_edges_list.clear(); - // Initialise the boundig box - this->u_min = I.getWidth(); - this->u_max = 0; - this->v_min = I.getHeight(); - this->v_max = 0; + // Initialise the bounding box + m_u_min = I.getWidth(); + m_u_max = 0; + m_v_min = I.getHeight(); + m_v_max = 0; -#if 0 - // Original version - if (connexe(I, (unsigned int)u, (unsigned int)v, - gray_level_min, gray_level_max, - mean_gray_level, u_cog, v_cog, npoint) == vpDot::out) { - bool sol = false; - unsigned int pas; - for (pas = 2; pas <= 25; ++pas)if (sol==false) { - for (int k = -1; k <=1; ++k) if (sol==false) - for (int l = -1; l <=1; ++l) if (sol==false) { - u_cog = 0; - v_cog = 0; - ip_connexities_list.clear(); - - this->mean_gray_level = 0; - if (connexe(I, (unsigned int)(u+k*pas), (unsigned int)(v+l*pas), - gray_level_min, gray_level_max, - mean_gray_level, u_cog, v_cog, npoint) != vpDot::out) { - sol = true; u += k*pas; v += l*pas; - } - } - } - if (sol == false) { - //vpERROR_TRACE("Dot has been lost") ; - throw(vpTrackingException(vpTrackingException::featureLostError, - "Dot has been lost")); - } - } -#else // If the dot is not found, search around using a spiral - if (!connexe(I, static_cast(u), static_cast(v), mean_gray_level, u_cog, v_cog, npoint)) { + if (!connexe(I, static_cast(u), static_cast(v), m_mean_gray_level, uv_cog, npoints)) { bool sol = false; unsigned int right = 1; @@ -486,18 +462,17 @@ void vpDot::COG(const vpImage &I, double &u, double &v) unsigned int up = 2; double u_ = u, v_ = v; unsigned int k; - + const unsigned int val_2 = 2; // Spiral search from the center to find the nearest dot while ((right < SPIRAL_SEARCH_SIZE) && (sol == false)) { for (k = 1; k <= right; ++k) { if (sol == false) { - u_cog = 0; - v_cog = 0; - ip_connexities_list.clear(); - ip_edges_list.clear(); + uv_cog.set_uv(0, 0); + m_ip_connexities_list.clear(); + m_ip_edges_list.clear(); - this->mean_gray_level = 0; - if (connexe(I, static_cast(u_) + k, static_cast(v_), mean_gray_level, u_cog, v_cog, npoint)) { + m_mean_gray_level = 0; + if (connexe(I, static_cast(u_) + k, static_cast(v_), m_mean_gray_level, uv_cog, npoints)) { sol = true; u = u_ + k; v = v_; @@ -505,18 +480,17 @@ void vpDot::COG(const vpImage &I, double &u, double &v) } } u_ += k; - right += 2; + right += val_2; for (k = 1; k <= botom; ++k) { if (sol == false) { - u_cog = 0; - v_cog = 0; - ip_connexities_list.clear(); - ip_edges_list.clear(); + uv_cog.set_uv(0, 0); + m_ip_connexities_list.clear(); + m_ip_edges_list.clear(); - this->mean_gray_level = 0; + m_mean_gray_level = 0; - if (connexe(I, static_cast(u_), static_cast(v_ + k), mean_gray_level, u_cog, v_cog, npoint)) { + if (connexe(I, static_cast(u_), static_cast(v_ + k), m_mean_gray_level, uv_cog, npoints)) { sol = true; u = u_; v = v_ + k; @@ -524,18 +498,17 @@ void vpDot::COG(const vpImage &I, double &u, double &v) } } v_ += k; - botom += 2; + botom += val_2; for (k = 1; k <= left; ++k) { if (sol == false) { - u_cog = 0; - v_cog = 0; - ip_connexities_list.clear(); - ip_edges_list.clear(); + uv_cog.set_uv(0, 0); + m_ip_connexities_list.clear(); + m_ip_edges_list.clear(); - this->mean_gray_level = 0; + m_mean_gray_level = 0; - if (connexe(I, static_cast(u_ - k), static_cast(v_), mean_gray_level, u_cog, v_cog, npoint)) { + if (connexe(I, static_cast(u_ - k), static_cast(v_), m_mean_gray_level, uv_cog, npoints)) { sol = true; u = u_ - k; v = v_; @@ -543,18 +516,17 @@ void vpDot::COG(const vpImage &I, double &u, double &v) } } u_ -= k; - left += 2; + left += val_2; for (k = 1; k <= up; ++k) { if (sol == false) { - u_cog = 0; - v_cog = 0; - ip_connexities_list.clear(); - ip_edges_list.clear(); + uv_cog.set_uv(0, 0); + m_ip_connexities_list.clear(); + m_ip_edges_list.clear(); - this->mean_gray_level = 0; + m_mean_gray_level = 0; - if (connexe(I, static_cast(u_), static_cast(v_ - k), mean_gray_level, u_cog, v_cog, npoint)) { + if (connexe(I, static_cast(u_), static_cast(v_ - k), m_mean_gray_level, uv_cog, npoints)) { sol = true; u = u_; v = v_ - k; @@ -562,61 +534,49 @@ void vpDot::COG(const vpImage &I, double &u, double &v) } } v_ -= k; - up += 2; + up += val_2; } if (sol == false) { - // vpERROR_TRACE("Dot has been lost") ; throw(vpTrackingException(vpTrackingException::featureLostError, "Dot has been lost")); } } -#endif - /* - vpImagePoint ip; - unsigned int i, j; - std::list::iterator it; - for (it = ip_connexities_list.begin(); it != ip_connexities_list.end(); it - ++) { ip = *it; i = (unsigned int) ip.get_i(); j = (unsigned int) - ip.get_j(); I[i][j] = 255 ; - }*/ + uv_cog.set_u(uv_cog.get_u() / npoints); + uv_cog.set_v(uv_cog.get_v() / npoints); - u_cog = u_cog / npoint; - v_cog = v_cog / npoint; - - u = u_cog; - v = v_cog; + u = uv_cog.get_u(); + v = uv_cog.get_v(); + const unsigned int val_max = 255; // Initialize the threshold for the next call to track() - double Ip = pow((double)this->mean_gray_level / 255, 1 / gamma); + double Ip = pow(static_cast(m_mean_gray_level) / val_max, 1 / m_gamma); - if ((Ip - (1 - grayLevelPrecision)) < 0) { - gray_level_min = 0; + if ((Ip - (1 - m_grayLevelPrecision)) < 0) { + m_gray_level_min = 0; } else { - gray_level_min = static_cast(255 * pow(Ip - (1 - grayLevelPrecision), gamma)); - if (gray_level_min > 255) { - gray_level_min = 255; + m_gray_level_min = static_cast(val_max * pow(Ip - (1 - m_grayLevelPrecision), m_gamma)); + if (m_gray_level_min > val_max) { + m_gray_level_min = val_max; } } - gray_level_max = static_cast(255 * pow(Ip + (1 - grayLevelPrecision), gamma)); - if (gray_level_max > 255) { - gray_level_max = 255; + m_gray_level_max = static_cast(val_max * pow(Ip + (1 - m_grayLevelPrecision), m_gamma)); + if (m_gray_level_max > val_max) { + m_gray_level_max = val_max; } - // --comment: trace the gray level - - if (npoint < 5) { - // --comment: trace the error using vpERROR_TRACE "Dot to small" + const double nbMinPoint = 5; + if (npoints < nbMinPoint) { throw(vpTrackingException(vpTrackingException::featureLostError, "Dot to small")); } - if (npoint > nbMaxPoint) { + if (npoints > m_nbMaxPoint) { throw(vpTrackingException(vpTrackingException::featureLostError, "Too many point %lf (%lf%%). Max allowed is " - "%lf (%lf%%). This threshold can be modified " + "%u (%f%%). This threshold can be modified " "using the setMaxDotSize() method.", - npoint, npoint / (I.getWidth() * I.getHeight()), nbMaxPoint, maxDotSizePercentage)); + npoints, static_cast(npoints) / (I.getWidth() * I.getHeight()), m_nbMaxPoint, m_maxDotSizePercentage)); } } @@ -636,11 +596,11 @@ void vpDot::setMaxDotSize(double percentage) { if ((percentage <= 0.0) || (percentage > 1.0)) { // print a warning. We keep the default percentage - vpTRACE("Max dot size percentage is requested to be set to %lf.", - "Value should be in ]0:1]. Value will be set to %lf.", percentage, maxDotSizePercentage); + std::cout << "Max dot size percentage is requested to be set to " << percentage << "." << std::endl; + std::cout << "Value should be in ]0:1]. Value will be set to " << m_maxDotSizePercentage << "." << std::endl; } else { - maxDotSizePercentage = percentage; + m_maxDotSizePercentage = percentage; } } @@ -652,7 +612,7 @@ void vpDot::setMaxDotSize(double percentage) Wait a user click in a gray area in the image I. The clicked pixel will be the starting point from which the dot will be tracked. - The threshold used to segment the dot is set with the grayLevelPrecision + The threshold used to segment the dot is set with the gray level precision parameter. See the formula in setGrayLevelPrecision() function. The sub pixel coordinates of the dot are updated. To get the center @@ -669,26 +629,28 @@ void vpDot::setMaxDotSize(double percentage) */ void vpDot::initTracking(const vpImage &I) { - while (vpDisplay::getClick(I, cog) != true) - ; + while (vpDisplay::getClick(I, m_cog) != true) { + // Wait until a click is detected + } - unsigned int i = static_cast(cog.get_i()); - unsigned int j = static_cast(cog.get_j()); + unsigned int i = static_cast(m_cog.get_i()); + unsigned int j = static_cast(m_cog.get_j()); + const unsigned int val_max = 255; - double Ip = pow((double)I[i][j] / 255, 1 / gamma); + double Ip = pow(static_cast(I[i][j]) / val_max, 1 / m_gamma); - if ((Ip - (1 - grayLevelPrecision)) < 0) { - gray_level_min = 0; + if ((Ip - (1 - m_grayLevelPrecision)) < 0) { + m_gray_level_min = 0; } else { - gray_level_min = static_cast(255 * pow(Ip - (1 - grayLevelPrecision), gamma)); - if (gray_level_min > 255) { - gray_level_min = 255; + m_gray_level_min = static_cast(val_max * pow(Ip - (1 - m_grayLevelPrecision), m_gamma)); + if (m_gray_level_min > val_max) { + m_gray_level_min = val_max; } } - gray_level_max = static_cast(255 * pow(Ip + (1 - grayLevelPrecision), gamma)); - if (gray_level_max > 255) { - gray_level_max = 255; + m_gray_level_max = static_cast(val_max * pow(Ip + (1 - m_grayLevelPrecision), m_gamma)); + if (m_gray_level_max > val_max) { + m_gray_level_max = val_max; } track(I); @@ -719,26 +681,25 @@ void vpDot::initTracking(const vpImage &I) */ void vpDot::initTracking(const vpImage &I, const vpImagePoint &ip) { + m_cog = ip; - cog = ip; - - unsigned int i = static_cast(cog.get_i()); - unsigned int j = static_cast(cog.get_j()); + unsigned int i = static_cast(m_cog.get_i()); + unsigned int j = static_cast(m_cog.get_j()); + const unsigned int val_max = 255; + double Ip = pow(static_cast(I[i][j]) / val_max, 1 / m_gamma); - double Ip = pow((double)I[i][j] / 255, 1 / gamma); - - if ((Ip - (1 - grayLevelPrecision)) < 0) { - gray_level_min = 0; + if ((Ip - (1 - m_grayLevelPrecision)) < 0) { + m_gray_level_min = 0; } else { - gray_level_min = static_cast(255 * pow(Ip - (1 - grayLevelPrecision), gamma)); - if (gray_level_min > 255) { - gray_level_min = 255; + m_gray_level_min = static_cast(val_max * pow(Ip - (1 - m_grayLevelPrecision), m_gamma)); + if (m_gray_level_min > val_max) { + m_gray_level_min = val_max; } } - gray_level_max = static_cast(255 * pow(Ip + (1 - grayLevelPrecision), gamma)); - if (gray_level_max > 255) { - gray_level_max = 255; + m_gray_level_max = static_cast(val_max * pow(Ip + (1 - m_grayLevelPrecision), m_gamma)); + if (m_gray_level_max > val_max) { + m_gray_level_max = val_max; } track(I); @@ -774,11 +735,10 @@ void vpDot::initTracking(const vpImage &I, const vpImagePoint &ip void vpDot::initTracking(const vpImage &I, const vpImagePoint &ip, unsigned int level_min, unsigned int level_max) { + m_cog = ip; - cog = ip; - - this->gray_level_min = level_min; - this->gray_level_max = level_max; + m_gray_level_min = level_min; + m_gray_level_max = level_max; track(I); } @@ -800,23 +760,25 @@ void vpDot::initTracking(const vpImage &I, const vpImagePoint &ip void vpDot::track(const vpImage &I) { setGrayLevelOut(); - double u = this->cog.get_u(); - double v = this->cog.get_v(); + double u = m_cog.get_u(); + double v = m_cog.get_v(); COG(I, u, v); - this->cog.set_u(u); - this->cog.set_v(v); + m_cog.set_u(u); + m_cog.set_v(v); - if (compute_moment == true) { + if (m_compute_moment == true) { mu11 = m11 - (u * m01); mu02 = m02 - (v * m01); mu20 = m20 - (u * m10); } - if (graphics) { + if (m_graphics) { + const unsigned int val_3 = 3; + const unsigned int val_8 = 8; // display a red cross at the center of gravity's location in the image. - vpDisplay::displayCross(I, this->cog, (3 * thickness) + 8, vpColor::red, thickness); + vpDisplay::displayCross(I, m_cog, (val_3 * m_thickness) + val_8, vpColor::red, m_thickness); } } @@ -838,7 +800,7 @@ void vpDot::track(const vpImage &I, vpImagePoint &ip) { track(I); - ip = this->cog; + ip = m_cog; } /*! @@ -850,11 +812,13 @@ void vpDot::track(const vpImage &I, vpImagePoint &ip) */ void vpDot::display(const vpImage &I, vpColor color, unsigned int thick) const { - vpDisplay::displayCross(I, cog, (3 * thickness) + 8, color, thick); + const unsigned int val_3 = 3; + const unsigned int val_8 = 8; + vpDisplay::displayCross(I, m_cog, (val_3 * m_thickness) + val_8, color, thick); std::list::const_iterator it; - std::list::const_iterator ip_edges_list_end = ip_edges_list.end(); - for (it = ip_edges_list.begin(); it != ip_edges_list_end; ++it) { + std::list::const_iterator m_ip_edges_list_end = m_ip_edges_list.end(); + for (it = m_ip_edges_list.begin(); it != m_ip_edges_list_end; ++it) { vpDisplay::displayPoint(I, *it, color); } } @@ -867,7 +831,7 @@ void vpDot::display(const vpImage &I, vpColor color, unsigned int in ]0,1]: - 1 means full precision, whereas values close to 0 show a very bad accuracy. - - Values lower or equal to 0 are brought back to an epsion>0 + - Values lower or equal to 0 are brought back to an epsilon>0 - Values higher than 1 are brought back to 1 If the initial gray level is I, the gray levels of the dot will be between : \f$Imin=255*\big((\frac{I}{255})^{{\gamma}^{-1}}-(1-grayLevelPrecision)\big)^{\gamma}\f$ @@ -880,14 +844,14 @@ void vpDot::display(const vpImage &I, vpColor color, unsigned int void vpDot::setGrayLevelPrecision(const double &precision) { double epsilon = 0.05; - if (grayLevelPrecision < epsilon) { - this->grayLevelPrecision = epsilon; + if (m_grayLevelPrecision < epsilon) { + m_grayLevelPrecision = epsilon; } - else if (grayLevelPrecision > 1) { - this->grayLevelPrecision = 1.0; + else if (m_grayLevelPrecision > 1) { + m_grayLevelPrecision = 1.0; } else { - this->grayLevelPrecision = precision; + m_grayLevelPrecision = precision; } } @@ -908,7 +872,9 @@ void vpDot::setGrayLevelPrecision(const double &precision) void vpDot::display(const vpImage &I, const vpImagePoint &cog, const std::list &edges_list, vpColor color, unsigned int thickness) { - vpDisplay::displayCross(I, cog, (3 * thickness) + 8, color, thickness); + const unsigned int val_3 = 3; + const unsigned int val_8 = 8; + vpDisplay::displayCross(I, cog, (val_3 * thickness) + val_8, color, thickness); std::list::const_iterator it; std::list::const_iterator edges_list_end = edges_list.end(); @@ -934,7 +900,9 @@ void vpDot::display(const vpImage &I, const vpImagePoint &cog, co void vpDot::display(const vpImage &I, const vpImagePoint &cog, const std::list &edges_list, vpColor color, unsigned int thickness) { - vpDisplay::displayCross(I, cog, (3 * thickness) + 8, color, thickness); + const unsigned int val_3 = 3; + const unsigned int val_8 = 8; + vpDisplay::displayCross(I, cog, (val_3 * thickness) + val_8, color, thickness); std::list::const_iterator it; std::list::const_iterator edges_list_end = edges_list.end(); diff --git a/modules/tracker/blob/src/dots/vpDot2.cpp b/modules/tracker/blob/src/dots/vpDot2.cpp index 8098cb65ff..e28047e842 100644 --- a/modules/tracker/blob/src/dots/vpDot2.cpp +++ b/modules/tracker/blob/src/dots/vpDot2.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,7 @@ * * Description: * Track a white dot. - * - * Authors: - * Anthony Saunier - * -*****************************************************************************/ + */ /*! \file vpDot2.cpp @@ -68,21 +63,23 @@ BEGIN_VISP_NAMESPACE */ void vpDot2::init() { - cog.set_u(0); - cog.set_v(0); - - width = 0; - height = 0; - surface = 0; - mean_gray_level = 0; - gray_level_min = 128; - gray_level_max = 255; - grayLevelPrecision = 0.80; - gamma = 1.5; - - sizePrecision = 0.65; - ellipsoidShapePrecision = 0.65; - maxSizeSearchDistancePrecision = 0.65; + const unsigned int val_max = 255; + const unsigned int val_median = 128; + m_cog.set_u(0); + m_cog.set_v(0); + + m_width = 0; + m_height = 0; + m_surface = 0; + m_mean_gray_level = 0; + m_gray_level_min = val_median; + m_gray_level_max = val_max; + m_grayLevelPrecision = 0.80; + m_gamma = 1.5; + + m_sizePrecision = 0.65; + m_ellipsoidShapePrecision = 0.65; + m_maxSizeSearchDistPrecision = 0.65; setEllipsoidBadPointsPercentage(); m00 = 0.; m11 = 0.; @@ -94,29 +91,34 @@ void vpDot2::init() mu02 = 0.; mu20 = 0.; - bbox_u_min = 0; - bbox_u_max = 0; - bbox_v_min = 0; - bbox_v_max = 0; + m_bbox_u_min = 0; + m_bbox_u_max = 0; + m_bbox_v_min = 0; + m_bbox_v_max = 0; - firstBorder_u = 0; - firstBorder_v = 0; + m_firstBorder_u = 0; + m_firstBorder_v = 0; - compute_moment = false; - graphics = false; - thickness = 1; + m_compute_moment = false; + m_graphics = false; + m_thickness = 1; } /*! Default constructor. Just do basic default initialization. */ vpDot2::vpDot2() - : m00(0.), m10(0.), m01(0.), m11(0.), m20(0.), m02(0.), mu11(0.), mu20(0.), mu02(0.), cog(), width(0), height(0), - surface(0), gray_level_min(128), gray_level_max(255), mean_gray_level(0), grayLevelPrecision(0.8), gamma(1.5), - sizePrecision(0.65), ellipsoidShapePrecision(0.65), maxSizeSearchDistancePrecision(0.65), - allowedBadPointsPercentage_(0.), area(), direction_list(), ip_edges_list(), compute_moment(false), graphics(false), - thickness(1), bbox_u_min(0), bbox_u_max(0), bbox_v_min(0), bbox_v_max(0), firstBorder_u(0), firstBorder_v() -{ } + : m00(0.), m10(0.), m01(0.), m11(0.), m20(0.), m02(0.), mu11(0.), mu20(0.), mu02(0.), m_cog(), m_width(0), m_height(0), + m_surface(0), m_mean_gray_level(0), m_grayLevelPrecision(0.8), m_gamma(1.5), + m_sizePrecision(0.65), m_ellipsoidShapePrecision(0.65), m_maxSizeSearchDistPrecision(0.65), + m_allowedBadPointsPercentage(0.), m_area(), m_direction_list(), m_ip_edges_list(), m_compute_moment(false), m_graphics(false), + m_thickness(1), m_bbox_u_min(0), m_bbox_u_max(0), m_bbox_v_min(0), m_bbox_v_max(0), m_firstBorder_u(0), m_firstBorder_v() +{ + const unsigned int val_max = 255; + const unsigned int val_median = 128; + m_gray_level_min = val_median; + m_gray_level_max = val_max; +} /*! @@ -127,24 +129,33 @@ vpDot2::vpDot2() */ vpDot2::vpDot2(const vpImagePoint &ip) - : m00(0.), m10(0.), m01(0.), m11(0.), m20(0.), m02(0.), mu11(0.), mu20(0.), mu02(0.), cog(ip), width(0), height(0), - surface(0), gray_level_min(128), gray_level_max(255), mean_gray_level(0), grayLevelPrecision(0.8), gamma(1.5), - sizePrecision(0.65), ellipsoidShapePrecision(0.65), maxSizeSearchDistancePrecision(0.65), - allowedBadPointsPercentage_(0.), area(), direction_list(), ip_edges_list(), compute_moment(false), graphics(false), - thickness(1), bbox_u_min(0), bbox_u_max(0), bbox_v_min(0), bbox_v_max(0), firstBorder_u(0), firstBorder_v() -{ } + : m00(0.), m10(0.), m01(0.), m11(0.), m20(0.), m02(0.), mu11(0.), mu20(0.), mu02(0.), m_cog(ip), m_width(0), m_height(0), + m_surface(0), m_mean_gray_level(0), m_grayLevelPrecision(0.8), m_gamma(1.5), + m_sizePrecision(0.65), m_ellipsoidShapePrecision(0.65), m_maxSizeSearchDistPrecision(0.65), + m_allowedBadPointsPercentage(0.), m_area(), m_direction_list(), m_ip_edges_list(), m_compute_moment(false), m_graphics(false), + m_thickness(1), m_bbox_u_min(0), m_bbox_u_max(0), m_bbox_v_min(0), m_bbox_v_max(0), m_firstBorder_u(0), m_firstBorder_v() +{ + const unsigned int val_max = 255; + const unsigned int val_median = 128; + m_gray_level_min = val_median; + m_gray_level_max = val_max; +} /*! Copy constructor. */ vpDot2::vpDot2(const vpDot2 &twinDot) - : vpTracker(twinDot), m00(0.), m10(0.), m01(0.), m11(0.), m20(0.), m02(0.), mu11(0.), mu20(0.), mu02(0.), cog(), - width(0), height(0), surface(0), gray_level_min(128), gray_level_max(255), mean_gray_level(0), - grayLevelPrecision(0.8), gamma(1.5), sizePrecision(0.65), ellipsoidShapePrecision(0.65), - maxSizeSearchDistancePrecision(0.65), allowedBadPointsPercentage_(0.), area(), direction_list(), ip_edges_list(), - compute_moment(false), graphics(false), thickness(1), bbox_u_min(0), bbox_u_max(0), bbox_v_min(0), bbox_v_max(0), - firstBorder_u(0), firstBorder_v() + : vpTracker(twinDot), m00(0.), m10(0.), m01(0.), m11(0.), m20(0.), m02(0.), mu11(0.), mu20(0.), mu02(0.), m_cog(), + m_width(0), m_height(0), m_surface(0), m_mean_gray_level(0), + m_grayLevelPrecision(0.8), m_gamma(1.5), m_sizePrecision(0.65), m_ellipsoidShapePrecision(0.65), + m_maxSizeSearchDistPrecision(0.65), m_allowedBadPointsPercentage(0.), m_area(), m_direction_list(), m_ip_edges_list(), + m_compute_moment(false), m_graphics(false), m_thickness(1), m_bbox_u_min(0), m_bbox_u_max(0), m_bbox_v_min(0), m_bbox_v_max(0), + m_firstBorder_u(0), m_firstBorder_v() { + const unsigned int val_max = 255; + const unsigned int val_median = 128; + m_gray_level_min = val_median; + m_gray_level_max = val_max; *this = twinDot; } @@ -153,37 +164,37 @@ vpDot2::vpDot2(const vpDot2 &twinDot) */ vpDot2 &vpDot2::operator=(const vpDot2 &twinDot) { - cog = twinDot.cog; - - width = twinDot.width; - height = twinDot.height; - surface = twinDot.surface; - gray_level_min = twinDot.gray_level_min; - gray_level_max = twinDot.gray_level_max; - mean_gray_level = twinDot.mean_gray_level; - grayLevelPrecision = twinDot.grayLevelPrecision; - gamma = twinDot.gamma; - - sizePrecision = twinDot.sizePrecision; - ellipsoidShapePrecision = twinDot.ellipsoidShapePrecision; - maxSizeSearchDistancePrecision = twinDot.maxSizeSearchDistancePrecision; - allowedBadPointsPercentage_ = twinDot.allowedBadPointsPercentage_; - area = twinDot.area; - - direction_list = twinDot.direction_list; - ip_edges_list = twinDot.ip_edges_list; - - compute_moment = twinDot.compute_moment; - graphics = twinDot.graphics; - thickness = twinDot.thickness; - - bbox_u_min = twinDot.bbox_u_min; - bbox_u_max = twinDot.bbox_u_max; - bbox_v_min = twinDot.bbox_v_min; - bbox_v_max = twinDot.bbox_v_max; - - firstBorder_u = twinDot.firstBorder_u; - firstBorder_v = twinDot.firstBorder_v; + m_cog = twinDot.m_cog; + + m_width = twinDot.m_width; + m_height = twinDot.m_height; + m_surface = twinDot.m_surface; + m_gray_level_min = twinDot.m_gray_level_min; + m_gray_level_max = twinDot.m_gray_level_max; + m_mean_gray_level = twinDot.m_mean_gray_level; + m_grayLevelPrecision = twinDot.m_grayLevelPrecision; + m_gamma = twinDot.m_gamma; + + m_sizePrecision = twinDot.m_sizePrecision; + m_ellipsoidShapePrecision = twinDot.m_ellipsoidShapePrecision; + m_maxSizeSearchDistPrecision = twinDot.m_maxSizeSearchDistPrecision; + m_allowedBadPointsPercentage = twinDot.m_allowedBadPointsPercentage; + m_area = twinDot.m_area; + + m_direction_list = twinDot.m_direction_list; + m_ip_edges_list = twinDot.m_ip_edges_list; + + m_compute_moment = twinDot.m_compute_moment; + m_graphics = twinDot.m_graphics; + m_thickness = twinDot.m_thickness; + + m_bbox_u_min = twinDot.m_bbox_u_min; + m_bbox_u_max = twinDot.m_bbox_u_max; + m_bbox_v_min = twinDot.m_bbox_v_min; + m_bbox_v_max = twinDot.m_bbox_v_max; + + m_firstBorder_u = twinDot.m_firstBorder_u; + m_firstBorder_v = twinDot.m_firstBorder_v; m00 = twinDot.m00; m01 = twinDot.m01; @@ -213,11 +224,13 @@ vpDot2 &vpDot2::operator=(const vpDot2 &twinDot) */ void vpDot2::display(const vpImage &I, vpColor color, unsigned int t) const { - vpDisplay::displayCross(I, cog, (3 * t) + 8, color, t); + const unsigned int val_3 = 3; + const unsigned int val_8 = 8; + vpDisplay::displayCross(I, m_cog, (val_3 * t) + val_8, color, t); std::list::const_iterator it; - std::list::const_iterator ip_edges_list_end = ip_edges_list.end(); - for (it = ip_edges_list.begin(); it != ip_edges_list_end; ++it) { + std::list::const_iterator ip_edges_list_end = m_ip_edges_list.end(); + for (it = m_ip_edges_list.begin(); it != ip_edges_list_end; ++it) { vpDisplay::displayPoint(I, *it, color); } } @@ -255,27 +268,28 @@ void vpDot2::display(const vpImage &I, vpColor color, unsigned in */ void vpDot2::initTracking(const vpImage &I, unsigned int size) { - while (vpDisplay::getClick(I, cog) != true) { + while (vpDisplay::getClick(I, m_cog) != true) { // block empty waiting user interaction } - unsigned int i = static_cast(cog.get_i()); - unsigned int j = static_cast(cog.get_j()); + unsigned int i = static_cast(m_cog.get_i()); + unsigned int j = static_cast(m_cog.get_j()); + const unsigned int val_max = 255; - double Ip = pow(static_cast(I[i][j]) / 255, 1 / gamma); + double Ip = pow(static_cast(I[i][j]) / val_max, 1 / m_gamma); - if ((Ip - (1 - grayLevelPrecision)) < 0) { - gray_level_min = 0; + if ((Ip - (1 - m_grayLevelPrecision)) < 0) { + m_gray_level_min = 0; } else { - gray_level_min = static_cast(255 * pow(Ip - (1 - grayLevelPrecision), gamma)); - if (gray_level_min > 255) { - gray_level_min = 255; + m_gray_level_min = static_cast(val_max * pow(Ip - (1 - m_grayLevelPrecision), m_gamma)); + if (m_gray_level_min > val_max) { + m_gray_level_min = val_max; } } - gray_level_max = static_cast(255 * pow(Ip + (1 - grayLevelPrecision), gamma)); - if (gray_level_max > 255) { - gray_level_max = 255; + m_gray_level_max = static_cast(val_max * pow(Ip + (1 - m_grayLevelPrecision), m_gamma)); + if (m_gray_level_max > val_max) { + m_gray_level_max = val_max; } setWidth(size); @@ -313,25 +327,26 @@ void vpDot2::initTracking(const vpImage &I, unsigned int size) */ void vpDot2::initTracking(const vpImage &I, const vpImagePoint &ip, unsigned int size) { - cog = ip; + m_cog = ip; - unsigned int i = static_cast(cog.get_i()); - unsigned int j = static_cast(cog.get_j()); + unsigned int i = static_cast(m_cog.get_i()); + unsigned int j = static_cast(m_cog.get_j()); + const unsigned int val_max = 255; - double Ip = pow(static_cast(I[i][j]) / 255, 1 / gamma); + double Ip = pow(static_cast(I[i][j]) / val_max, 1 / m_gamma); - if ((Ip - (1 - grayLevelPrecision)) < 0) { - gray_level_min = 0; + if ((Ip - (1 - m_grayLevelPrecision)) < 0) { + m_gray_level_min = 0; } else { - gray_level_min = static_cast(255 * pow(Ip - (1 - grayLevelPrecision), gamma)); - if (gray_level_min > 255) { - gray_level_min = 255; + m_gray_level_min = static_cast(val_max * pow(Ip - (1 - m_grayLevelPrecision), m_gamma)); + if (m_gray_level_min > val_max) { + m_gray_level_min = val_max; } } - gray_level_max = static_cast(255 * pow(Ip + (1 - grayLevelPrecision), gamma)); - if (gray_level_max > 255) { - gray_level_max = 255; + m_gray_level_max = static_cast(val_max * pow(Ip + (1 - m_grayLevelPrecision), m_gamma)); + if (m_gray_level_max > val_max) { + m_gray_level_max = val_max; } setWidth(size); @@ -382,10 +397,10 @@ void vpDot2::initTracking(const vpImage &I, const vpImagePoint &i void vpDot2::initTracking(const vpImage &I, const vpImagePoint &ip, unsigned int gray_lvl_min, unsigned int gray_lvl_max, unsigned int size) { - cog = ip; + m_cog = ip; - this->gray_level_min = gray_lvl_min; - this->gray_level_max = gray_lvl_max; + m_gray_level_min = gray_lvl_min; + m_gray_level_max = gray_lvl_max; setWidth(size); setHeight(size); @@ -455,7 +470,7 @@ void vpDot2::track(const vpImage &I, bool canMakeTheWindowGrow) // this copy to set the current found dot to the previous one (see below). vpDot2 wantedDot(*this); - bool found = computeParameters(I, cog.get_u(), cog.get_v()); + bool found = computeParameters(I, m_cog.get_u(), m_cog.get_v()); if (found) { // test if the found dot is valid (ie similar to the previous one) @@ -467,11 +482,6 @@ void vpDot2::track(const vpImage &I, bool canMakeTheWindowGrow) } if (!found) { - // vpDEBUG_TRACE(0, "Search the dot in a biggest window around the - // last position"); vpDEBUG_TRACE(0, "Bad computed dot: "); - // vpDEBUG_TRACE(0, "u: %f v: %f", get_u(), get_v()); - // vpDEBUG_TRACE(0, "w: %f h: %f", getWidth(), getHeight()); - // if estimation was wrong (get an error tracking), look for the dot // closest from the estimation, // i.e. search for dots in an a region of interest around the this dot and @@ -487,8 +497,9 @@ void vpDot2::track(const vpImage &I, bool canMakeTheWindowGrow) searchWindowHeight = 80.; } else if (canMakeTheWindowGrow) { - searchWindowWidth = getWidth() * 5; - searchWindowHeight = getHeight() * 5; + const unsigned int val_5 = 5; + searchWindowWidth = getWidth() * val_5; + searchWindowHeight = getHeight() * val_5; } else { searchWindowWidth = getWidth(); @@ -496,15 +507,14 @@ void vpDot2::track(const vpImage &I, bool canMakeTheWindowGrow) } std::list candidates; - searchDotsInArea(I, static_cast(this->cog.get_u() - (searchWindowWidth / 2.0)), - static_cast(this->cog.get_v() - (searchWindowHeight / 2.0)), + searchDotsInArea(I, static_cast(m_cog.get_u() - (searchWindowWidth / 2.0)), + static_cast(m_cog.get_v() - (searchWindowHeight / 2.0)), static_cast(searchWindowWidth), static_cast(searchWindowHeight), candidates); // if the vector is empty, that mean we didn't find any candidate // in the area, return an error tracking. if (candidates.empty()) { - // vpERROR_TRACE("No dot was found") ; throw(vpTrackingException(vpTrackingException::featureLostError, "No dot was found")); } @@ -525,45 +535,42 @@ void vpDot2::track(const vpImage &I, bool canMakeTheWindowGrow) m02 = movingDot.m02; // Update the bounding box - bbox_u_min = movingDot.bbox_u_min; - bbox_u_max = movingDot.bbox_u_max; - bbox_v_min = movingDot.bbox_v_min; - bbox_v_max = movingDot.bbox_v_max; + m_bbox_u_min = movingDot.m_bbox_u_min; + m_bbox_u_max = movingDot.m_bbox_u_max; + m_bbox_v_min = movingDot.m_bbox_v_min; + m_bbox_v_max = movingDot.m_bbox_v_max; } // if this dot is partially out of the image, return an error tracking. if (!isInImage(I)) { - // vpERROR_TRACE("The center of gravity of the dot is not in the image") ; throw(vpTrackingException(vpTrackingException::featureLostError, "The center of gravity of the dot is not in the image")); } - // Get dots center of gravity - // unsigned int u = (unsigned int) this->cog.get_u(); - // unsigned int v = (unsigned int) this->cog.get_v(); - // Updates the min and max gray levels for the next iteration - // double Ip = pow((double)I[v][u]/255,1/gamma); - double Ip = pow(getMeanGrayLevel() / 255, 1 / gamma); + const unsigned int val_max = 255; + double Ip = pow(getMeanGrayLevel() / val_max, 1 / m_gamma); // printf("current value of gray level center : %i\n", I[v][u]); // get Mean Gray Level of I - if ((Ip - (1 - grayLevelPrecision)) < 0) { - gray_level_min = 0; + if ((Ip - (1 - m_grayLevelPrecision)) < 0) { + m_gray_level_min = 0; } else { - gray_level_min = static_cast(255 * pow(Ip - (1 - grayLevelPrecision), gamma)); - if (gray_level_min > 255) { - gray_level_min = 255; + m_gray_level_min = static_cast(val_max * pow(Ip - (1 - m_grayLevelPrecision), m_gamma)); + if (m_gray_level_min > val_max) { + m_gray_level_min = val_max; } } - gray_level_max = static_cast(255 * pow(Ip + (1 - grayLevelPrecision), gamma)); - if (gray_level_max > 255) { - gray_level_max = 255; + m_gray_level_max = static_cast(val_max * pow(Ip + (1 - m_grayLevelPrecision), m_gamma)); + if (m_gray_level_max > val_max) { + m_gray_level_max = val_max; } - if (graphics) { + if (m_graphics) { // display a red cross at the center of gravity's location in the image. - vpDisplay::displayCross(I, this->cog, (3 * thickness) + 8, vpColor::red, thickness); + const unsigned int val_3 = 3; + const unsigned int val_8 = 8; + vpDisplay::displayCross(I, m_cog, (val_3 * m_thickness) + val_8, vpColor::red, m_thickness); } } @@ -593,7 +600,7 @@ void vpDot2::track(const vpImage &I, vpImagePoint &ip, bool canMa { track(I, canMakeTheWindowGrow); - ip = this->cog; + ip = m_cog; } ///// GET METHODS @@ -604,35 +611,35 @@ void vpDot2::track(const vpImage &I, vpImagePoint &ip, bool canMa \sa getHeight() */ -double vpDot2::getWidth() const { return width; } +double vpDot2::getWidth() const { return m_width; } /*! Return the height of the dot. \sa getWidth() */ -double vpDot2::getHeight() const { return height; } +double vpDot2::getHeight() const { return m_height; } /*! Return the area of the dot. The area of the dot is also given by \f$|m00|\f$. */ -double vpDot2::getArea() const { return fabs(surface); } +double vpDot2::getArea() const { return fabs(m_surface); } /*! Return the precision of the gray level of the dot. It is a double precision float which value is in [0,1]. 1 means full precision, whereas values close to 0 show a very bad precision. */ -double vpDot2::getGrayLevelPrecision() const { return grayLevelPrecision; } +double vpDot2::getGrayLevelPrecision() const { return m_grayLevelPrecision; } /*! Return the precision of the size of the dot. It is a double precision float which value is in [0.05,1]. 1 means full precision, whereas values close to 0 show a very bad precision. */ -double vpDot2::getSizePrecision() const { return sizePrecision; } +double vpDot2::getSizePrecision() const { return m_sizePrecision; } /*! Return the precision of the ellipsoid shape of the dot. It is a double @@ -641,7 +648,7 @@ double vpDot2::getSizePrecision() const { return sizePrecision; } \sa setEllipsoidShapePrecision() */ -double vpDot2::getEllipsoidShapePrecision() const { return ellipsoidShapePrecision; } +double vpDot2::getEllipsoidShapePrecision() const { return m_ellipsoidShapePrecision; } /*! Return the precision of the search maximum distance to get the starting @@ -649,7 +656,7 @@ double vpDot2::getEllipsoidShapePrecision() const { return ellipsoidShapePrecisi [0.05,1]. 1 means full precision, whereas values close to 0 show a very bad precision. */ -double vpDot2::getMaxSizeSearchDistPrecision() const { return maxSizeSearchDistancePrecision; } +double vpDot2::getMaxSizeSearchDistPrecision() const { return m_maxSizeSearchDistPrecision; } /*! Return the distance between the two center of dots. @@ -657,8 +664,8 @@ double vpDot2::getMaxSizeSearchDistPrecision() const { return maxSizeSearchDista double vpDot2::getDistance(const vpDot2 &distantDot) const { vpImagePoint cogDistantDot = distantDot.getCog(); - double diff_u = this->cog.get_u() - cogDistantDot.get_u(); - double diff_v = this->cog.get_v() - cogDistantDot.get_v(); + double diff_u = m_cog.get_u() - cogDistantDot.get_u(); + double diff_v = m_cog.get_v() - cogDistantDot.get_v(); return sqrt((diff_u * diff_u) + (diff_v * diff_v)); } @@ -673,7 +680,7 @@ double vpDot2::getDistance(const vpDot2 &distantDot) const \sa setHeight(), setArea(), setSizePrecision() */ -void vpDot2::setWidth(const double &w) { this->width = w; } +void vpDot2::setWidth(const double &w) { m_width = w; } /*! @@ -685,7 +692,7 @@ void vpDot2::setWidth(const double &w) { this->width = w; } \sa setWidth(), setArea(), setSizePrecision() */ -void vpDot2::setHeight(const double &h) { this->height = h; } +void vpDot2::setHeight(const double &h) { m_height = h; } /*! @@ -697,7 +704,7 @@ void vpDot2::setHeight(const double &h) { this->height = h; } \sa setWidth(), setHeight(), setSizePrecision() */ -void vpDot2::setArea(const double &a) { this->surface = a; } +void vpDot2::setArea(const double &a) { m_surface = a; } /*! @@ -719,14 +726,14 @@ void vpDot2::setArea(const double &a) { this->surface = a; } void vpDot2::setGrayLevelPrecision(const double &precision) { double epsilon = 0.05; - if (grayLevelPrecision < epsilon) { - this->grayLevelPrecision = epsilon; + if (m_grayLevelPrecision < epsilon) { + m_grayLevelPrecision = epsilon; } - else if (grayLevelPrecision > 1) { - this->grayLevelPrecision = 1.0; + else if (m_grayLevelPrecision > 1) { + m_grayLevelPrecision = 1.0; } else { - this->grayLevelPrecision = precision; + m_grayLevelPrecision = precision; } } /*! @@ -748,14 +755,14 @@ void vpDot2::setGrayLevelPrecision(const double &precision) */ void vpDot2::setSizePrecision(const double &precision) { - if (sizePrecision < 0) { - this->sizePrecision = 0; + if (m_sizePrecision < 0) { + m_sizePrecision = 0; } - else if (sizePrecision > 1) { - this->sizePrecision = 1.0; + else if (m_sizePrecision > 1) { + m_sizePrecision = 1.0; } else { - this->sizePrecision = precision; + m_sizePrecision = precision; } } @@ -794,14 +801,14 @@ void vpDot2::setSizePrecision(const double &precision) void vpDot2::setEllipsoidShapePrecision(const double &precision) { - if (ellipsoidShapePrecision < 0) { - this->ellipsoidShapePrecision = 0; + if (m_ellipsoidShapePrecision < 0) { + m_ellipsoidShapePrecision = 0; } - else if (ellipsoidShapePrecision > 1) { - this->ellipsoidShapePrecision = 1.0; + else if (m_ellipsoidShapePrecision > 1) { + m_ellipsoidShapePrecision = 1.0; } else { - this->ellipsoidShapePrecision = precision; + m_ellipsoidShapePrecision = precision; } } @@ -823,14 +830,14 @@ void vpDot2::setEllipsoidShapePrecision(const double &precision) void vpDot2::setMaxSizeSearchDistPrecision(const double &precision) { double epsilon = 0.05; - if (maxSizeSearchDistancePrecision < epsilon) { - this->maxSizeSearchDistancePrecision = epsilon; + if (m_maxSizeSearchDistPrecision < epsilon) { + m_maxSizeSearchDistPrecision = epsilon; } - else if (maxSizeSearchDistancePrecision > 1) { - this->maxSizeSearchDistancePrecision = 1.0; + else if (m_maxSizeSearchDistPrecision > 1) { + m_maxSizeSearchDistPrecision = 1.0; } else { - this->maxSizeSearchDistancePrecision = precision; + m_maxSizeSearchDistPrecision = precision; } } @@ -882,7 +889,7 @@ void vpDot2::setArea(const vpImage &I, int u, int v, unsigned int h = image_h - static_cast(v) - 1; } - area.setRect(u, v, w, h); + m_area.setRect(u, v, w, h); } /*! @@ -892,7 +899,7 @@ void vpDot2::setArea(const vpImage &I, int u, int v, unsigned int \param a : Area. */ -void vpDot2::setArea(const vpRect &a) { area = a; } +void vpDot2::setArea(const vpRect &area) { m_area = area; } ///// CLASS FUNCTIONALITY //////////////////////////////////////////////////// @@ -952,6 +959,168 @@ void vpDot2::searchDotsInArea(const vpImage &I, std::list searchDotsInArea(I, 0, 0, I.getWidth(), I.getHeight(), niceDots); } +/** + * \brief Performs the research of dots in the area when the point u, v is a good germ. + * + * \param data The data required for the algorithm. + */ +void vpDot2::searchDotsAreaGoodGerm(vpSearchDotsInAreaGoodGermData &data) +{ +// Compute the right border position for this possible germ + unsigned int border_u; + unsigned int border_v; + bool good_germ = true; + std::list::iterator itbad; + std::list::iterator itnice; + vpDot2 *dotToTest = nullptr; + vpDot2 tmpDot; + vpImagePoint cogTmpDot; + if (findFirstBorder(data.m_I, data.m_u, data.m_v, border_u, border_v) == false) { + // germ is not good. + // Jump all the pixels between v,u and v, + // dotToTest->getFirstBorder_u() + data.m_u = border_u; + data.m_v = border_v; + } + else { + itbad = data.m_badDotsVector.begin(); + vpImagePoint cogBadDot; + + while ((itbad != data.m_badDotsVector.end()) && (good_germ == true)) { + if ((static_cast(data.m_u) >= (*itbad).m_bbox_u_min) && (static_cast(data.m_u) <= (*itbad).m_bbox_u_max) && + (static_cast(data.m_v) >= (*itbad).m_bbox_v_min) && (static_cast(data.m_v) <= (*itbad).m_bbox_v_max)) { + std::list::const_iterator it_edges = m_ip_edges_list.begin(); + while ((it_edges != m_ip_edges_list.end()) && (good_germ == true)) { + // Test if the germ belong to a previously detected dot: + // - from the germ go right to the border and compare this + // position to the list of pixels of previously detected dots + cogBadDot = *it_edges; + if ((std::fabs(border_u - cogBadDot.get_u()) <= + (vpMath::maximum(std::fabs(static_cast(border_u)), std::fabs(cogBadDot.get_u())) * + std::numeric_limits::epsilon())) && + (std::fabs(data.m_v - cogBadDot.get_v()) <= + (vpMath::maximum(std::fabs(static_cast(data.m_v)), std::fabs(cogBadDot.get_v())) * + std::numeric_limits::epsilon()))) { + good_germ = false; + } + ++it_edges; + } + } + ++itbad; + } + + if (!good_germ) { + // Jump all the pixels between v,u and v, + // dotToTest->getFirstBorder_u() + data.m_u = border_u; + data.m_v = border_v; + } + else { + vpImagePoint germ; + germ.set_u(data.m_u); + germ.set_v(data.m_v); + + // otherwise estimate the width, height and surface of the dot we + // created, and test it. + if (dotToTest != nullptr) { + delete dotToTest; + } + dotToTest = getInstance(); + dotToTest->setCog(germ); + dotToTest->setGrayLevelMin(getGrayLevelMin()); + dotToTest->setGrayLevelMax(getGrayLevelMax()); + dotToTest->setGrayLevelPrecision(getGrayLevelPrecision()); + dotToTest->setSizePrecision(getSizePrecision()); + dotToTest->setGraphics(m_graphics); + dotToTest->setGraphicsThickness(m_thickness); + dotToTest->setComputeMoments(true); + dotToTest->setArea(m_area); + dotToTest->setEllipsoidShapePrecision(m_ellipsoidShapePrecision); + dotToTest->setEllipsoidBadPointsPercentage(m_allowedBadPointsPercentage); + + // first compute the parameters of the dot. + // if for some reasons this caused an error tracking + // (dot partially out of the image...), check the next intersection + if (dotToTest->computeParameters(data.m_I) == false) { + // Jump all the pixels between v,u and v, + // dotToTest->getFirstBorder_u() + data.m_u = border_u; + data.m_v = border_v; + } + else { + // if the dot to test is valid, + if (dotToTest->isValid(data.m_I, *this)) { + vpImagePoint cogDotToTest = dotToTest->getCog(); + // Compute the distance to the center. The center used here is not the + // area center available by area.getCenter(area_center_u, + // area_center_v) but the center of the input area which may be + // partially outside the image. + + double area_center_u = (data.m_area.getLeft() + (data.m_area.getWidth() / 2.0)) - 0.5; + double area_center_v = (data.m_area.getTop() + (data.m_area.getHeight() / 2.0)) - 0.5; + + double thisDiff_u = cogDotToTest.get_u() - area_center_u; + double thisDiff_v = cogDotToTest.get_v() - area_center_v; + double thisDist = sqrt((thisDiff_u * thisDiff_u) + (thisDiff_v * thisDiff_v)); + + bool stopLoop = false; + itnice = data.m_niceDots.begin(); + + while ((itnice != data.m_niceDots.end()) && (stopLoop == false)) { + tmpDot = *itnice; + + // --comment: epsilon equals 0.001 -- detecte +sieurs points + double epsilon = 3.0; + // if the center of the dot is the same than the current + // don't add it, test the next point of the grid + cogTmpDot = tmpDot.getCog(); + + if ((fabs(cogTmpDot.get_u() - cogDotToTest.get_u()) < epsilon) && + (fabs(cogTmpDot.get_v() - cogDotToTest.get_v()) < epsilon)) { + stopLoop = true; + // Jump all the pixels between v,u and v, + // tmpDot->getFirstBorder_u() + data.m_u = border_u; + data.m_v = border_v; + } + else { + double otherDiff_u = cogTmpDot.get_u() - area_center_u; + double otherDiff_v = cogTmpDot.get_v() - area_center_v; + double otherDist = sqrt((otherDiff_u * otherDiff_u) + (otherDiff_v * otherDiff_v)); + + // if the distance of the curent vector element to the center + // is greater than the distance of this dot to the center, + // then add this dot before the current vector element. + if (otherDist > thisDist) { + data.m_niceDots.insert(itnice, *dotToTest); + stopLoop = true; + // Jump all the pixels between v,u and v, + // tmpDot->getFirstBorder_u() + data.m_u = border_u; + data.m_v = border_v; + } + ++itnice; + } + } + + // if we reached the end of the vector without finding the dot + // or inserting it, insert it now. + if ((itnice == data.m_niceDots.end()) && (stopLoop == false)) { + data.m_niceDots.push_back(*dotToTest); + } + } + else { + // Store bad dots + data.m_badDotsVector.push_front(*dotToTest); + } + } + } + } + if (dotToTest != nullptr) { + delete dotToTest; + } +} + /*! Look for a list of dot matching this dot parameters within a region of @@ -990,13 +1159,13 @@ void vpDot2::searchDotsInArea(const vpImage &I, int area_u, int a unsigned int gridHeight; getGridSize(gridWidth, gridHeight); - if (graphics) { + if (m_graphics) { // Display the area were the dot is search - vpDisplay::displayRectangle(I, area, vpColor::blue, false, thickness); + vpDisplay::displayRectangle(I, m_area, vpColor::blue, false, m_thickness); } #ifdef DEBUG - vpDisplay::displayRectangle(I, area, vpColor::blue); + vpDisplay::displayRectangle(I, m_area, vpColor::blue); vpDisplay::flush(I); #endif // start the search loop; for all points of the search grid, @@ -1004,207 +1173,56 @@ void vpDot2::searchDotsInArea(const vpImage &I, int area_u, int a // if it is so eventually add it to the vector of valid dots. std::list badDotsVector; std::list::iterator itnice; - std::list::iterator itbad; - vpDot2 *dotToTest = nullptr; vpDot2 tmpDot; - unsigned int area_u_min = static_cast(area.getLeft()); - unsigned int area_u_max = static_cast(area.getRight()); - unsigned int area_v_min = static_cast(area.getTop()); - unsigned int area_v_max = static_cast(area.getBottom()); + unsigned int area_u_min = static_cast(m_area.getLeft()); + unsigned int area_u_max = static_cast(m_area.getRight()); + unsigned int area_v_min = static_cast(m_area.getTop()); + unsigned int area_v_max = static_cast(m_area.getBottom()); unsigned int u, v; vpImagePoint cogTmpDot; - for (v = area_v_min; v < area_v_max; v = v + gridHeight) { - for (u = area_u_min; u < area_u_max; u = u + gridWidth) { + v = area_v_min; + while (v < area_v_max) { + u = area_u_min; + while (u < area_u_max) { // if the pixel we're in doesn't have the right color (outside the // graylevel interval), no need to check further, just get to the // next grid intersection. - if (!hasGoodLevel(I, u, v)) { - continue; - } - - // Test if an other germ is inside the bounding box of a dot previously - // detected - bool good_germ = true; + if (hasGoodLevel(I, u, v)) { - itnice = niceDots.begin(); - while ((itnice != niceDots.end()) && (good_germ == true)) { - tmpDot = *itnice; - - cogTmpDot = tmpDot.getCog(); - double u0 = cogTmpDot.get_u(); - double v0 = cogTmpDot.get_v(); - double half_w = tmpDot.getWidth() / 2.; - double half_h = tmpDot.getHeight() / 2.; - - if ((u >= (u0 - half_w)) && (u <= (u0 + half_w)) && (v >= (v0 - half_h)) && (v <= (v0 + half_h))) { - // Germ is in a previously detected dot - good_germ = false; - } - ++itnice; - } - - if (!good_germ) { - continue; - } - - // Compute the right border position for this possible germ - unsigned int border_u; - unsigned int border_v; - if (findFirstBorder(I, u, v, border_u, border_v) == false) { - // germ is not good. - // Jump all the pixels between v,u and v, - // dotToTest->getFirstBorder_u() - u = border_u; - v = border_v; - continue; - } + // Test if an other germ is inside the bounding box of a dot previously + // detected + bool good_germ = true; - itbad = badDotsVector.begin(); -#define vpBAD_DOT_VALUE (*itbad) - vpImagePoint cogBadDot; - - while ((itbad != badDotsVector.end()) && (good_germ == true)) { - if ((static_cast(u) >= vpBAD_DOT_VALUE.bbox_u_min) && (static_cast(u) <= vpBAD_DOT_VALUE.bbox_u_max) && - (static_cast(v) >= vpBAD_DOT_VALUE.bbox_v_min) && (static_cast(v) <= vpBAD_DOT_VALUE.bbox_v_max)) { - std::list::const_iterator it_edges = ip_edges_list.begin(); - while ((it_edges != ip_edges_list.end()) && (good_germ == true)) { - // Test if the germ belong to a previously detected dot: - // - from the germ go right to the border and compare this - // position to the list of pixels of previously detected dots - cogBadDot = *it_edges; - if ((std::fabs(border_u - cogBadDot.get_u()) <= - (vpMath::maximum(std::fabs(static_cast(border_u)), std::fabs(cogBadDot.get_u())) * - std::numeric_limits::epsilon())) && - (std::fabs(v - cogBadDot.get_v()) <= - (vpMath::maximum(std::fabs(static_cast(v)), std::fabs(cogBadDot.get_v())) * - std::numeric_limits::epsilon()))) { - good_germ = false; - } - ++it_edges; - } - } - ++itbad; - } -#undef vpBAD_DOT_VALUE - - if (!good_germ) { - // Jump all the pixels between v,u and v, - // dotToTest->getFirstBorder_u() - u = border_u; - v = border_v; - continue; - } - - vpTRACE(4, "Try germ (%d, %d)", u, v); - - vpImagePoint germ; - germ.set_u(u); - germ.set_v(v); - - // otherwise estimate the width, height and surface of the dot we - // created, and test it. - if (dotToTest != nullptr) { - delete dotToTest; - } - dotToTest = getInstance(); - dotToTest->setCog(germ); - dotToTest->setGrayLevelMin(getGrayLevelMin()); - dotToTest->setGrayLevelMax(getGrayLevelMax()); - dotToTest->setGrayLevelPrecision(getGrayLevelPrecision()); - dotToTest->setSizePrecision(getSizePrecision()); - dotToTest->setGraphics(graphics); - dotToTest->setGraphicsThickness(thickness); - dotToTest->setComputeMoments(true); - dotToTest->setArea(area); - dotToTest->setEllipsoidShapePrecision(ellipsoidShapePrecision); - dotToTest->setEllipsoidBadPointsPercentage(allowedBadPointsPercentage_); - - // first compute the parameters of the dot. - // if for some reasons this caused an error tracking - // (dot partially out of the image...), check the next intersection - if (dotToTest->computeParameters(I) == false) { - // Jump all the pixels between v,u and v, - // dotToTest->getFirstBorder_u() - u = border_u; - v = border_v; - continue; - } - // if the dot to test is valid, - if (dotToTest->isValid(I, *this)) { - vpImagePoint cogDotToTest = dotToTest->getCog(); - // Compute the distance to the center. The center used here is not the - // area center available by area.getCenter(area_center_u, - // area_center_v) but the center of the input area which may be - // partially outside the image. - - double area_center_u = (area_u + (area_w / 2.0)) - 0.5; - double area_center_v = (area_v + (area_h / 2.0)) - 0.5; - - double thisDiff_u = cogDotToTest.get_u() - area_center_u; - double thisDiff_v = cogDotToTest.get_v() - area_center_v; - double thisDist = sqrt((thisDiff_u * thisDiff_u) + (thisDiff_v * thisDiff_v)); - - bool stopLoop = false; itnice = niceDots.begin(); - - while ((itnice != niceDots.end()) && (stopLoop == false)) { + while ((itnice != niceDots.end()) && (good_germ == true)) { tmpDot = *itnice; - // --comment: epsilon equals 0.001 -- detecte +sieurs points - double epsilon = 3.0; - // if the center of the dot is the same than the current - // don't add it, test the next point of the grid cogTmpDot = tmpDot.getCog(); - - if ((fabs(cogTmpDot.get_u() - cogDotToTest.get_u()) < epsilon) && - (fabs(cogTmpDot.get_v() - cogDotToTest.get_v()) < epsilon)) { - stopLoop = true; - // Jump all the pixels between v,u and v, - // tmpDot->getFirstBorder_u() - u = border_u; - v = border_v; - continue; - } - - double otherDiff_u = cogTmpDot.get_u() - area_center_u; - double otherDiff_v = cogTmpDot.get_v() - area_center_v; - double otherDist = sqrt((otherDiff_u * otherDiff_u) + (otherDiff_v * otherDiff_v)); - - // if the distance of the curent vector element to the center - // is greater than the distance of this dot to the center, - // then add this dot before the current vector element. - if (otherDist > thisDist) { - niceDots.insert(itnice, *dotToTest); - ++itnice; - stopLoop = true; - // Jump all the pixels between v,u and v, - // tmpDot->getFirstBorder_u() - u = border_u; - v = border_v; - continue; + double u0 = cogTmpDot.get_u(); + double v0 = cogTmpDot.get_v(); + double half_w = tmpDot.getWidth() / 2.; + double half_h = tmpDot.getHeight() / 2.; + + if ((u >= (u0 - half_w)) && (u <= (u0 + half_w)) && (v >= (v0 - half_h)) && (v <= (v0 + half_h))) { + // Germ is in a previously detected dot + good_germ = false; } ++itnice; } - vpTRACE(4, "End while (%d, %d)", u, v); - // if we reached the end of the vector without finding the dot - // or inserting it, insert it now. - if ((itnice == niceDots.end()) && (stopLoop == false)) { - niceDots.push_back(*dotToTest); + if (good_germ) { + vpRect area(area_u, area_v, area_w, area_h); + vpSearchDotsInAreaGoodGermData data(I, area, u, v, niceDots, badDotsVector); + searchDotsAreaGoodGerm(data); } } - else { - // Store bad dots - badDotsVector.push_front(*dotToTest); - } + u = u + gridWidth; } - } - if (dotToTest != nullptr) { - delete dotToTest; + v = v + gridHeight; } } @@ -1255,54 +1273,48 @@ bool vpDot2::isValid(const vpImage &I, const vpDot2 &wantedDot) #endif if ((((wantedDot.getWidth() * size_precision) - epsilon) < getWidth()) == false) { - vpDEBUG_TRACE(3, "Bad width > for dot (%g, %g)", cog.get_u(), cog.get_v()); #ifdef DEBUG - printf("Bad width > for dot (%g, %g)\n", cog.get_u(), cog.get_v()); + printf("Bad width > for dot (%g, %g)\n", m_cog.get_u(), m_cog.get_v()); #endif return false; } if ((getWidth() < (wantedDot.getWidth() / (size_precision + epsilon))) == false) { - vpDEBUG_TRACE(3, "Bad width > for dot (%g, %g)", cog.get_u(), cog.get_v()); #ifdef DEBUG printf("Bad width %g > %g for dot (%g, %g)\n", getWidth(), wantedDot.getWidth() / (size_precision + epsilon), - cog.get_u(), cog.get_v()); + m_cog.get_u(), m_cog.get_v()); #endif return false; } if ((((wantedDot.getHeight() * size_precision) - epsilon) < getHeight()) == false) { - vpDEBUG_TRACE(3, "Bad height > for dot (%g, %g)", cog.get_u(), cog.get_v()); #ifdef DEBUG printf("Bad height %g > %g for dot (%g, %g)\n", wantedDot.getHeight() * size_precision - epsilon, getHeight(), - cog.get_u(), cog.get_v()); + m_cog.get_u(), m_cog.get_v()); #endif return false; } if ((getHeight() < (wantedDot.getHeight() / (size_precision + epsilon))) == false) { - vpDEBUG_TRACE(3, "Bad height > for dot (%g, %g)", cog.get_u(), cog.get_v()); #ifdef DEBUG printf("Bad height %g > %g for dot (%g, %g)\n", getHeight(), wantedDot.getHeight() / (size_precision + epsilon), - cog.get_u(), cog.get_v()); + m_cog.get_u(), m_cog.get_v()); #endif return false; } if ((((wantedDot.getArea() * (size_precision * size_precision)) - epsilon) < getArea()) == false) { - vpDEBUG_TRACE(3, "Bad surface > for dot (%g, %g)", cog.get_u(), cog.get_v()); #ifdef DEBUG printf("Bad surface %g > %g for dot (%g, %g)\n", - wantedDot.getArea() * (size_precision * size_precision) - epsilon, getArea(), cog.get_u(), cog.get_v()); + wantedDot.getArea() * (size_precision * size_precision) - epsilon, getArea(), m_cog.get_u(), m_cog.get_v()); #endif return false; } if ((getArea() < (wantedDot.getArea() / ((size_precision * size_precision) + epsilon))) == false) { - vpDEBUG_TRACE(3, "Bad surface > for dot (%g, %g)", cog.get_u(), cog.get_v()); #ifdef DEBUG printf("Bad surface %g < %g for dot (%g, %g)\n", getArea(), - wantedDot.getArea() / (size_precision * size_precision + epsilon), cog.get_u(), cog.get_v()); + wantedDot.getArea() / (size_precision * size_precision + epsilon), m_cog.get_u(), m_cog.get_v()); #endif return false; } @@ -1315,11 +1327,11 @@ bool vpDot2::isValid(const vpImage &I, const vpDot2 &wantedDot) // int nb_point_to_test = 20; // Nb points to test on inner and outside ellipsoid int nb_bad_points = 0; - int nb_max_bad_points = static_cast(nb_point_to_test * allowedBadPointsPercentage_); + int nb_max_bad_points = static_cast(nb_point_to_test * m_allowedBadPointsPercentage); double step_angle = (2 * M_PI) / nb_point_to_test; // --comment: if ellipsoidShape_precision diff 0 and compute_moment is true - if ((std::fabs(ellipsoidShape_precision) > std::numeric_limits::epsilon()) && compute_moment) { + if ((std::fabs(ellipsoidShape_precision) > std::numeric_limits::epsilon()) && m_compute_moment) { // Chaumette, Image Moments: A General and Useful Set of Features for Visual Servoing, TRO 2004, eq 15 /* @@ -1349,24 +1361,25 @@ bool vpDot2::isValid(const vpImage &I, const vpDot2 &wantedDot) double innerCoef = ellipsoidShape_precision; unsigned int u, v; - double cog_u = this->cog.get_u(); - double cog_v = this->cog.get_v(); + double cog_u = m_cog.get_u(); + double cog_v = m_cog.get_v(); + double val_2 = 2; vpImagePoint ip; nb_bad_points = 0; - for (double theta = 0.; theta < (2 * M_PI); theta += step_angle) { + for (double theta = 0.; theta < (val_2 * M_PI); theta += step_angle) { u = static_cast(cog_u + (innerCoef * ((a1 * cos(alpha) * cos(theta)) - (a2 * sin(alpha) * sin(theta))))); v = static_cast(cog_v + (innerCoef * ((a1 * sin(alpha) * cos(theta)) + (a2 * cos(alpha) * sin(theta))))); if (!this->hasGoodLevel(I, u, v)) { #ifdef DEBUG printf("Inner circle pixel (%u, %u) has bad level for dot (%g, %g): " "%d not in [%u, %u]\n", - u, v, cog_u, cog_v, I[v][u], gray_level_min, gray_level_max); + u, v, cog_u, cog_v, I[v][u], m_gray_level_min, m_gray_level_max); #endif ++nb_bad_points; } - if (graphics) { - for (unsigned int t = 0; t < thickness; ++t) { + if (m_graphics) { + for (unsigned int t = 0; t < m_thickness; ++t) { ip.set_u(u + t); ip.set_v(v); vpDisplay::displayPoint(I, ip, vpColor::green); @@ -1390,7 +1403,7 @@ bool vpDot2::isValid(const vpImage &I, const vpDot2 &wantedDot) double outCoef = 2 - ellipsoidShape_precision; // --comment: 1.6 nb_bad_points = 0; - for (double theta = 0.; theta < (2 * M_PI); theta += step_angle) { + for (double theta = 0.; theta < (val_2 * M_PI); theta += step_angle) { u = static_cast(cog_u + (outCoef * ((a1 * cos(alpha) * cos(theta)) - (a2 * sin(alpha) * sin(theta))))); v = static_cast(cog_v + (outCoef * ((a1 * sin(alpha) * cos(theta)) + (a2 * cos(alpha) * sin(theta))))); #ifdef DEBUG @@ -1399,10 +1412,10 @@ bool vpDot2::isValid(const vpImage &I, const vpDot2 &wantedDot) vpDisplay::flush(I); #endif // If outside the area, continue - if ((static_cast(u) < area.getLeft()) || - (static_cast(u) > area.getRight()) || - (static_cast(v) < area.getTop()) || - (static_cast(v) > area.getBottom())) { + if ((static_cast(u) < m_area.getLeft()) || + (static_cast(u) > m_area.getRight()) || + (static_cast(v) < m_area.getTop()) || + (static_cast(v) > m_area.getBottom())) { // continue } else { @@ -1410,12 +1423,12 @@ bool vpDot2::isValid(const vpImage &I, const vpDot2 &wantedDot) #ifdef DEBUG printf("Outside circle pixel (%u, %u) has bad level for dot (%g, " "%g): %d not in [%u, %u]\n", - u, v, cog_u, cog_v, I[v][u], gray_level_min, gray_level_max); + u, v, cog_u, cog_v, I[v][u], m_gray_level_min, m_gray_level_max); #endif ++nb_bad_points; } - if (graphics) { - for (unsigned int t = 0; t < thickness; ++t) { + if (m_graphics) { + for (unsigned int t = 0; t < m_thickness; ++t) { ip.set_u(u + t); ip.set_v(v); @@ -1459,7 +1472,7 @@ bool vpDot2::hasGoodLevel(const vpImage &I, const unsigned int &u return false; } - if ((I[v][u] >= gray_level_min) && (I[v][u] <= gray_level_max)) { + if ((I[v][u] >= m_gray_level_min) && (I[v][u] <= m_gray_level_max)) { return true; } else { @@ -1486,7 +1499,7 @@ bool vpDot2::hasReverseLevel(const vpImage &I, const unsigned int return false; } - if ((I[v][u] < gray_level_min) || (I[v][u] > gray_level_max)) { + if ((I[v][u] < m_gray_level_min) || (I[v][u] > m_gray_level_max)) { return true; } else { @@ -1519,7 +1532,7 @@ vpDot2 *vpDot2::getInstance() { return new vpDot2(); } - 6 : down - 7 : down right */ -void vpDot2::getFreemanChain(std::list &freeman_chain) const { freeman_chain = direction_list; } +void vpDot2::getFreemanChain(std::list &freeman_chain) const { freeman_chain = m_direction_list; } /****************************************************************************** * @@ -1560,8 +1573,8 @@ void vpDot2::getFreemanChain(std::list &freeman_chain) const { fre */ bool vpDot2::computeParameters(const vpImage &I, const double &v_u, const double &v_v) { - direction_list.clear(); - ip_edges_list.clear(); + m_direction_list.clear(); + m_ip_edges_list.clear(); double est_u = v_u; // estimated double est_v = v_v; @@ -1569,68 +1582,59 @@ bool vpDot2::computeParameters(const vpImage &I, const double &v_ // if u has default value, set it to the actual center value // if( est_u == -1.0 ) if (std::fabs(est_u + 1.0) <= (vpMath::maximum(std::fabs(est_u), 1.) * std::numeric_limits::epsilon())) { - est_u = this->cog.get_u(); + est_u = m_cog.get_u(); } // if v has default value, set it to the actual center value // if( est_v == -1.0 ) if (std::fabs(est_v + 1.0) <= (vpMath::maximum(std::fabs(est_v), 1.) * std::numeric_limits::epsilon())) { - est_v = this->cog.get_v(); + est_v = m_cog.get_v(); } // if the estimated position of the dot is out of the image, not need to // continue, return an error tracking if (!isInArea(static_cast(est_u), static_cast(est_v))) { - vpDEBUG_TRACE(3, - "Initial pixel coordinates (%d, %d) for dot tracking are " - "not in the area", - static_cast(est_u), static_cast(est_v)); return false; } - bbox_u_min = static_cast(I.getWidth()); - bbox_u_max = 0; - bbox_v_min = static_cast(I.getHeight()); - bbox_v_max = 0; + m_bbox_u_min = static_cast(I.getWidth()); + m_bbox_u_max = 0; + m_bbox_v_min = static_cast(I.getHeight()); + m_bbox_v_max = 0; // if the first point doesn't have the right level then there's no point to // continue. if (!hasGoodLevel(I, static_cast(est_u), static_cast(est_v))) { - vpDEBUG_TRACE(3, "Can't find a dot from pixel (%d, %d) coordinates", static_cast(est_u), static_cast(est_v)); return false; } // find the border - if (!findFirstBorder(I, static_cast(est_u), static_cast(est_v), this->firstBorder_u, this->firstBorder_v)) { - - vpDEBUG_TRACE(3, "Can't find first border (%d, %d) coordinates", static_cast(est_u), static_cast(est_v)); + if (!findFirstBorder(I, static_cast(est_u), static_cast(est_v), m_firstBorder_u, m_firstBorder_v)) { return false; } unsigned int dir = 6; // Determine the first element of the Freeman chain - computeFreemanChainElement(I, this->firstBorder_u, this->firstBorder_v, dir); + computeFreemanChainElement(I, m_firstBorder_u, m_firstBorder_v, dir); unsigned int firstDir = dir; // if we are now out of the image, return an error tracking - if (!isInArea(this->firstBorder_u, this->firstBorder_v)) { - vpDEBUG_TRACE(3, "Border pixel coordinates (%d, %d) of the dot are not in the area", this->firstBorder_u, - this->firstBorder_v); + if (!isInArea(m_firstBorder_u, m_firstBorder_v)) { return false; } // store the new direction and dot border coordinates. - direction_list.push_back(dir); + m_direction_list.push_back(dir); vpImagePoint ip; - ip.set_u(this->firstBorder_u); - ip.set_v(this->firstBorder_v); + ip.set_u(m_firstBorder_u); + ip.set_v(m_firstBorder_v); - ip_edges_list.push_back(ip); + m_ip_edges_list.push_back(ip); - int border_u = static_cast(this->firstBorder_u); - int border_v = static_cast(this->firstBorder_v); + int border_u = static_cast(m_firstBorder_u); + int border_v = static_cast(m_firstBorder_v); int du, dv; float dS, dMu, dMv, dMuv, dMu2, dMv2; m00 = 0.0; @@ -1642,8 +1646,8 @@ bool vpDot2::computeParameters(const vpImage &I, const double &v_ // while we didn't come back to the first point, follow the border do { // if it was asked, show the border - if (graphics) { - for (int t = 0; t < static_cast(thickness); ++t) { + if (m_graphics) { + for (int t = 0; t < static_cast(m_thickness); ++t) { ip.set_u(border_u + t); ip.set_v(border_v); @@ -1666,50 +1670,43 @@ bool vpDot2::computeParameters(const vpImage &I, const double &v_ m00 += dS; // enclosed area m10 += dMu; // First order moment along v axis m01 += dMv; // First order moment along u axis - if (compute_moment) { + if (m_compute_moment) { m11 += dMuv; // Second order moment m20 += dMu2; // Second order moment along v axis m02 += dMv2; // Second order moment along u axis } // if we are now out of the image, return an error tracking if (!isInArea(static_cast(border_u), static_cast(border_v))) { - - vpDEBUG_TRACE(3, "Dot (%d, %d) is not in the area", border_u, border_v); // Can Occur on a single pixel dot located on the top border return false; } // store the new direction and dot border coordinates. - direction_list.push_back(dir); + m_direction_list.push_back(dir); ip.set_u(border_u); ip.set_v(border_v); - ip_edges_list.push_back(ip); + m_ip_edges_list.push_back(ip); // update the extreme point of the dot. - if (border_v < bbox_v_min) { - bbox_v_min = border_v; + if (border_v < m_bbox_v_min) { + m_bbox_v_min = border_v; } - if (border_v > bbox_v_max) { - bbox_v_max = border_v; + if (border_v > m_bbox_v_max) { + m_bbox_v_max = border_v; } - if (border_u < bbox_u_min) { - bbox_u_min = border_u; + if (border_u < m_bbox_u_min) { + m_bbox_u_min = border_u; } - if (border_u > bbox_u_max) { - bbox_u_max = border_u; + if (border_u > m_bbox_u_max) { + m_bbox_u_max = border_u; } // move around the tracked entity by following the border. if (computeFreemanChainElement(I, static_cast(border_u), static_cast(border_v), dir) == false) { - vpDEBUG_TRACE(3, "Can't compute Freeman chain for dot (%d, %d)", border_u, border_v); return false; } - - // vpTRACE("border_u: %d border_v: %d dir: %d", border_u, border_v, - // dir); - } while (((getFirstBorder_u() != static_cast(border_u)) || (getFirstBorder_v() != static_cast(border_v)) || (firstDir != dir)) && isInArea(static_cast(border_u), static_cast(border_v))); @@ -1725,7 +1722,6 @@ bool vpDot2::computeParameters(const vpImage &I, const double &v_ // if( m00 == 0 || m00 == 1 ) if ((std::fabs(m00) <= std::numeric_limits::epsilon()) || (std::fabs(m00 - 1.) <= (vpMath::maximum(std::fabs(m00), 1.) * std::numeric_limits::epsilon()))) { - vpDEBUG_TRACE(3, "The center of gravity of the dot wasn't properly detected"); return false; } else // compute the center @@ -1735,19 +1731,19 @@ bool vpDot2::computeParameters(const vpImage &I, const double &v_ double tmpCenter_v = m01 / m00; // Updates the second order centered moments - if (compute_moment) { + if (m_compute_moment) { mu11 = m11 - (tmpCenter_u * m01); mu02 = m02 - (tmpCenter_v * m01); mu20 = m20 - (tmpCenter_u * m10); } - cog.set_u(tmpCenter_u); - cog.set_v(tmpCenter_v); + m_cog.set_u(tmpCenter_u); + m_cog.set_v(tmpCenter_v); } - width = (bbox_u_max - bbox_u_min) + 1; - height = (bbox_v_max - bbox_v_min) + 1; - surface = m00; + m_width = (m_bbox_u_max - m_bbox_u_min) + 1; + m_height = (m_bbox_v_max - m_bbox_v_min) + 1; + m_surface = m00; computeMeanGrayLevel(I); return true; @@ -1775,28 +1771,24 @@ bool vpDot2::findFirstBorder(const vpImage &I, const unsigned int // NOTE: // from here we use int and not double. This is because we don't have - // rounding problems and it's actually more a trouble than smth else to + // rounding problems and it's actually more a trouble than something else to // work with double when navigating around the dot. border_u = u; border_v = v; double epsilon = 0.001; #ifdef DEBUG - std::cout << "gray level: " << gray_level_min << " " << gray_level_max << std::endl; + std::cout << "gray level: " << m_gray_level_min << " " << m_gray_level_max << std::endl; #endif - while (hasGoodLevel(I, border_u + 1, border_v) && (border_u < area.getRight()) /*I.getWidth()*/) { + while (hasGoodLevel(I, border_u + 1, border_v) && (border_u < m_area.getRight()) /*I.getWidth()*/) { // if the width of this dot was initialised and we already crossed the dot // on more than the max possible width, no need to continue, return an // error tracking if ((getWidth() > 0) && ((border_u - u) > ((getWidth() / getMaxSizeSearchDistPrecision()) + epsilon))) { - vpDEBUG_TRACE(3, - "The found dot (%d, %d, %d) has a greater width than the " - "required one", - u, v, border_u); return false; } #ifdef DEBUG - vpDisplay::displayPoint(I, (int)border_v, (int)border_u + 1, vpColor::green); + vpDisplay::displayPoint(I, static_cast(border_v), static_cast(border_u) + 1, vpColor::green); vpDisplay::flush(I); #endif @@ -1826,22 +1818,29 @@ bool vpDot2::findFirstBorder(const vpImage &I, const unsigned int bool vpDot2::computeFreemanChainElement(const vpImage &I, const unsigned int &u, const unsigned int &v, unsigned int &element) { - if (hasGoodLevel(I, u, v)) { unsigned int v_u = u; unsigned int v_v = v; + const unsigned int val_1 = 1; + const unsigned int val_2 = 2; + const unsigned int val_3 = 3; + const unsigned int val_4 = 4; + const unsigned int val_5 = 5; + const unsigned int val_6 = 6; + const unsigned int val_7 = 7; + const unsigned int val_8 = 8; // get the point on the right of the point passed in - updateFreemanPosition(v_u, v_v, (element + 2) % 8); + updateFreemanPosition(v_u, v_v, (element + val_2) % val_8); if (hasGoodLevel(I, v_u, v_v)) { - element = (element + 2) % 8; // turn right + element = (element + val_2) % val_8; // turn right } else { unsigned int v_u1 = u; unsigned int v_v1 = v; - updateFreemanPosition(v_u1, v_v1, (element + 1) % 8); + updateFreemanPosition(v_u1, v_v1, (element + val_1) % val_8); if (hasGoodLevel(I, v_u1, v_v1)) { - element = (element + 1) % 8; // turn diag right + element = (element + val_1) % val_8; // turn diag right } else { unsigned int v_u2 = u; @@ -1854,42 +1853,42 @@ bool vpDot2::computeFreemanChainElement(const vpImage &I, const u else { unsigned int v_u3 = u; unsigned int v_v3 = v; - updateFreemanPosition(v_u3, v_v3, (element + 7) % 8); // diag left + updateFreemanPosition(v_u3, v_v3, (element + val_7) % val_8); // diag left if (hasGoodLevel(I, v_u3, v_v3)) { - element = (element + 7) % 8; // turn diag left + element = (element + val_7) % val_8; // turn diag left } else { unsigned int v_u4 = u; unsigned int v_v4 = v; - updateFreemanPosition(v_u4, v_v4, (element + 6) % 8); // left + updateFreemanPosition(v_u4, v_v4, (element + val_6) % val_8); // left if (hasGoodLevel(I, v_u4, v_v4)) { - element = (element + 6) % 8; // turn left + element = (element + val_6) % val_8; // turn left } else { unsigned int v_u5 = u; unsigned int v_v5 = v; - updateFreemanPosition(v_u5, v_v5, (element + 5) % 8); // left + updateFreemanPosition(v_u5, v_v5, (element + val_5) % val_8); // left if (hasGoodLevel(I, v_u5, v_v5)) { - element = (element + 5) % 8; // turn diag down + element = (element + val_5) % val_8; // turn diag down } else { unsigned int v_u6 = u; unsigned int v_v6 = v; - updateFreemanPosition(v_u6, v_v6, (element + 4) % 8); // left + updateFreemanPosition(v_u6, v_v6, (element + val_4) % val_8); // left if (hasGoodLevel(I, v_u6, v_v6)) { - element = (element + 4) % 8; // turn down + element = (element + val_4) % val_8; // turn down } else { unsigned int v_u7 = u; unsigned int v_v7 = v; - updateFreemanPosition(v_u7, v_v7, (element + 3) % 8); // diag + updateFreemanPosition(v_u7, v_v7, (element + val_3) % val_8); // diag if (hasGoodLevel(I, v_u7, v_v7)) { - element = (element + 3) % 8; // turn diag right down + element = (element + val_3) % val_8; // turn diag right down } else { // No neighbor with a good level @@ -1912,6 +1911,89 @@ bool vpDot2::computeFreemanChainElement(const vpImage &I, const u return true; } +namespace +{ +/*! +* Given the previous position of a pixel (u_p, v_p) on the dot border, the +* direction to reach the next pixel on the border being top right and +* because with use the moment, compute Freeman parameters. +* \param u_p : Previous value of the row coordinate of a pixel on a border. +* \param v_p : Previous value of the column coordinate of a pixel on a border. +* \param dMuv : Moment increases. Cumulated values of dMuv gives m11. +* \param dMu2 : Second order moment along v axis increases. Cumulated values +* of dMu2 gives m20. +* \param dMv2 : Second order moment along u axis increases. Cumulated values +* of dMv2 gives m02. +*/ +void computeTopRightWithMoment(const int &u_p, const int &v_p, float &dMuv, float &dMu2, float &dMv2) +{ + float half_u_p = static_cast(0.5 * u_p); + dMuv = static_cast((v_p * v_p * (0.25 + half_u_p)) + (v_p * ((1. / 3.) + half_u_p)) + ((1. / 6.) * u_p) + 0.125); + dMu2 = static_cast(((-1. / 3.) * u_p * ((u_p * u_p) + (1.5 * u_p) + 1.)) - (1. / 12.0)); + dMv2 = static_cast(((1. / 3.) * v_p * ((v_p * v_p) + (1.5 * v_p) + 1.)) + (1. / 12.0)); +} + +/*! +* Given the previous position of a pixel (u_p, v_p) on the dot border, the +* direction to reach the next pixel on the border being top left and +* because with use the moment, compute Freeman parameters. +* \param u_p : Previous value of the row coordinate of a pixel on a border. +* \param v_p : Previous value of the column coordinate of a pixel on a border. +* \param dMuv : Moment increases. Cumulated values of dMuv gives m11. +* \param dMu2 : Second order moment along v axis increases. Cumulated values +* of dMu2 gives m20. +* \param dMv2 : Second order moment along u axis increases. Cumulated values +* of dMv2 gives m02. +*/ +void computeTopLeftWithMoment(const int &u_p, const int &v_p, float &dMuv, float &dMu2, float &dMv2) +{ + float half_u_p = static_cast(0.5 * u_p); + dMuv = static_cast((((v_p * v_p * (0.25 - half_u_p)) + (v_p * ((1. / 3.) - half_u_p))) - ((1. / 6.) * u_p)) + 0.125); + dMu2 = static_cast(((-1. / 3.) * u_p * (((u_p * u_p) - (1.5 * u_p)) + 1.)) - (1. / 12.0)); + dMv2 = static_cast(((-1. / 3.) * v_p * ((v_p * v_p) + (1.5 * v_p) + 1.)) - (1. / 12.0)); +} + +/*! +* Given the previous position of a pixel (u_p, v_p) on the dot border, the +* direction to reach the next pixel on the border being down right and +* because with use the moment, compute Freeman parameters. +* \param u_p : Previous value of the row coordinate of a pixel on a border. +* \param v_p : Previous value of the column coordinate of a pixel on a border. +* \param dMuv : Moment increases. Cumulated values of dMuv gives m11. +* \param dMu2 : Second order moment along v axis increases. Cumulated values +* of dMu2 gives m20. +* \param dMv2 : Second order moment along u axis increases. Cumulated values +* of dMv2 gives m02. +*/ +void computeDownRightWithMoment(const int &u_p, const int &v_p, float &dMuv, float &dMu2, float &dMv2) +{ + float half_u_p = static_cast(0.5 * u_p); + dMuv = static_cast(((v_p * v_p * (0.25 + half_u_p)) - (v_p * ((1. / 3.) + half_u_p))) + ((1. / 6.) * u_p) + 0.125); + dMu2 = static_cast(((1. / 3.) * u_p * ((u_p * u_p) + (1.5 * u_p) + 1.)) + (1. / 12.0)); + dMv2 = static_cast(((1. / 3.) * v_p * (((v_p * v_p) - (1.5 * v_p)) + 1.)) - (1. / 12.0)); +} + +/*! +* Given the previous position of a pixel (u_p, v_p) on the dot border, the +* direction to reach the next pixel on the border being down left and +* because with use the moment, compute Freeman parameters. +* \param u_p : Previous value of the row coordinate of a pixel on a border. +* \param v_p : Previous value of the column coordinate of a pixel on a border. +* \param dMuv : Moment increases. Cumulated values of dMuv gives m11. +* \param dMu2 : Second order moment along v axis increases. Cumulated values +* of dMu2 gives m20. +* \param dMv2 : Second order moment along u axis increases. Cumulated values +* of dMv2 gives m02. +*/ +void computeDownLeftWithMoment(const int &u_p, const int &v_p, float &dMuv, float &dMu2, float &dMv2) +{ + float half_u_p = static_cast(0.5 * u_p); + dMuv = static_cast((((v_p * v_p * (0.25 - half_u_p)) - (v_p * ((1. / 3.) - half_u_p))) - ((1. / 6.) * u_p)) + 0.125); + dMu2 = static_cast(((1. / 3.) * u_p * (((u_p * u_p) - (1.5 * u_p)) + 1.)) - (1. / 12.0)); + dMv2 = static_cast(((-1. / 3.) * v_p * (((v_p * v_p) - (1.5 * v_p)) + 1.)) - (1. / 12.0)); +} +} + /*! Given the previous position of a pixel (u_p, v_p) on the dot border and the @@ -1944,123 +2026,119 @@ bool vpDot2::computeFreemanChainElement(const vpImage &I, const u void vpDot2::computeFreemanParameters(const int &u_p, const int &v_p, unsigned int &element, int &du, int &dv, float &dS, float &dMu, float &dMv, float &dMuv, float &dMu2, float &dMv2) { + /* + 3 2 1 + \ | / + \|/ + 4 ------- 0 + /|\ + / | \ + 5 6 7 + */ + const unsigned int go_right = 0; + const unsigned int go_right_top = 1; + const unsigned int go_top = 2; + const unsigned int go_top_left = 3; + const unsigned int go_left = 4; + const unsigned int go_left_down = 5; + const unsigned int go_down = 6; + const unsigned int go_down_right = 7; du = 0; dv = 0; dMuv = 0; dMu2 = 0; dMv2 = 0; - - /* - 3 2 1 - \ | / - \|/ - 4 ------- 0 - /|\ - / | \ - 5 6 7 - */ + const unsigned int val_2 = 2; switch (element) { - case 0: // go right + case go_right: // go right du = 1; dS = static_cast(v_p); dMu = 0.0; dMv = static_cast(0.5 * v_p * v_p); - if (compute_moment) { - dMuv = static_cast(0.25 * v_p * v_p * ((2 * u_p) + 1)); + if (m_compute_moment) { + dMuv = static_cast(0.25 * v_p * v_p * ((val_2 * u_p) + 1)); dMu2 = 0; dMv2 = static_cast((1.0 / 3.) * v_p * v_p * v_p); } break; - case 1: // go right top + case go_right_top: // go right top du = 1; dv = 1; dS = static_cast(v_p + 0.5); dMu = -static_cast((0.5 * u_p * (u_p + 1)) + (1.0 / 6.0)); dMv = static_cast((0.5 * v_p * (v_p + 1)) + (1.0 / 6.0)); - if (compute_moment) { - float half_u_p = static_cast(0.5 * u_p); - dMuv = static_cast((v_p * v_p * (0.25 + half_u_p)) + (v_p * ((1. / 3.) + half_u_p)) + ((1. / 6.) * u_p) + 0.125); - dMu2 = static_cast(((-1. / 3.) * u_p * ((u_p * u_p) + (1.5 * u_p) + 1.)) - (1. / 12.0)); - dMv2 = static_cast(((1. / 3.) * v_p * ((v_p * v_p) + (1.5 * v_p) + 1.)) + (1. / 12.0)); + if (m_compute_moment) { + computeTopRightWithMoment(u_p, v_p, dMuv, dMu2, dMv2); } break; - case 2: // go top + case go_top: // go top dv = 1; dS = 0.0; dMu = static_cast(-0.5 * u_p * u_p); dMv = 0.0; - if (compute_moment) { + if (m_compute_moment) { dMuv = 0; dMu2 = static_cast((-1.0 / 3.) * u_p * u_p * u_p); dMv2 = 0; } break; - case 3: + case go_top_left: du = -1; dv = 1; dS = static_cast(-v_p - 0.5); dMu = -static_cast((0.5 * u_p * (u_p - 1)) + (1.0 / 6.0)); dMv = -static_cast((0.5 * v_p * (v_p + 1)) + (1.0 / 6.0)); - if (compute_moment) { - float half_u_p = static_cast(0.5 * u_p); - dMuv = static_cast((((v_p * v_p * (0.25 - half_u_p)) + (v_p * ((1. / 3.) - half_u_p))) - ((1. / 6.) * u_p)) + 0.125); - dMu2 = static_cast(((-1. / 3.) * u_p * (((u_p * u_p) - (1.5 * u_p)) + 1.)) - (1. / 12.0)); - dMv2 = static_cast(((-1. / 3.) * v_p * ((v_p * v_p) + (1.5 * v_p) + 1.)) - (1. / 12.0)); + if (m_compute_moment) { + computeTopLeftWithMoment(u_p, v_p, dMuv, dMu2, dMv2); } break; - case 4: + case go_left: du = -1; dS = static_cast(-v_p); dMv = static_cast(-0.5 * v_p * v_p); dMu = 0.0; - if (compute_moment) { - dMuv = static_cast(-0.25 * v_p * v_p * ((2 * u_p) - 1)); + if (m_compute_moment) { + dMuv = static_cast(-0.25 * v_p * v_p * ((val_2 * u_p) - 1)); dMu2 = 0; dMv2 = static_cast((-1.0 / 3.) * v_p * v_p * v_p); } break; - case 5: + case go_left_down: du = -1; dv = -1; dS = static_cast(-v_p + 0.5); dMu = static_cast((0.5 * u_p * (u_p - 1)) + (1.0 / 6.0)); dMv = static_cast(-((0.5 * v_p * (v_p - 1)) + (1.0 / 6.0))); - if (compute_moment) { - float half_u_p = static_cast(0.5 * u_p); - dMuv = static_cast((((v_p * v_p * (0.25 - half_u_p)) - (v_p * ((1. / 3.) - half_u_p))) - ((1. / 6.) * u_p)) + 0.125); - dMu2 = static_cast(((1. / 3.) * u_p * (((u_p * u_p) - (1.5 * u_p)) + 1.)) - (1. / 12.0)); - dMv2 = static_cast(((-1. / 3.) * v_p * (((v_p * v_p) - (1.5 * v_p)) + 1.)) - (1. / 12.0)); + if (m_compute_moment) { + computeDownLeftWithMoment(u_p, v_p, dMuv, dMu2, dMv2); } break; - case 6: + case go_down: dv = -1; dS = 0.0; dMu = static_cast(0.5 * u_p * u_p); dMv = 0.0; - if (compute_moment) { + if (m_compute_moment) { dMuv = 0; dMu2 = static_cast((1.0 / 3.) * u_p * u_p * u_p); dMv2 = 0; } break; - case 7: + case go_down_right: du = 1; dv = -1; dS = static_cast(v_p - 0.5); dMu = static_cast((0.5 * u_p * (u_p + 1)) + (1.0 / 6.0)); dMv = static_cast((0.5 * v_p * (v_p - 1)) + (1.0 / 6.0)); - if (compute_moment) { - float half_u_p = static_cast(0.5 * u_p); - dMuv = static_cast(((v_p * v_p * (0.25 + half_u_p)) - (v_p * ((1. / 3.) + half_u_p))) + ((1. / 6.) * u_p) + 0.125); - dMu2 = static_cast(((1. / 3.) * u_p * ((u_p * u_p) + (1.5 * u_p) + 1.)) + (1. / 12.0)); - dMv2 = static_cast(((1. / 3.) * v_p * (((v_p * v_p) - (1.5 * v_p)) + 1.)) - (1. / 12.0)); + if (m_compute_moment) { + computeDownRightWithMoment(u_p, v_p, dMuv, dMu2, dMv2); } break; @@ -2084,32 +2162,49 @@ void vpDot2::computeFreemanParameters(const int &u_p, const int &v_p, unsigned i */ void vpDot2::updateFreemanPosition(unsigned int &u, unsigned int &v, const unsigned int &dir) { + /* + 3 2 1 + \ | / + \|/ + 4 ------- 0 + /|\ + / | \ + 5 6 7 + */ + const unsigned int go_right = 0; + const unsigned int go_right_top = 1; + const unsigned int go_top = 2; + const unsigned int go_top_left = 3; + const unsigned int go_left = 4; + const unsigned int go_left_down = 5; + const unsigned int go_down = 6; + const unsigned int go_down_right = 7; switch (dir) { - case 0: + case go_right: u += 1; break; - case 1: + case go_right_top: u += 1; v += 1; break; - case 2: + case go_top: v += 1; break; - case 3: + case go_top_left: u -= 1; v += 1; break; - case 4: + case go_left: u -= 1; break; - case 5: + case go_left_down: u -= 1; v -= 1; break; - case 6: + case go_down: v -= 1; break; - case 7: + case go_down_right: u += 1; v -= 1; break; @@ -2129,7 +2224,7 @@ void vpDot2::updateFreemanPosition(unsigned int &u, unsigned int &v, const unsig \return true if the pixel of coordinates (posI, posJ) is in the image and false otherwise. */ -bool vpDot2::isInImage(const vpImage &I) const { return isInImage(I, cog); } +bool vpDot2::isInImage(const vpImage &I) const { return isInImage(I, m_cog); } /*! @@ -2171,10 +2266,10 @@ bool vpDot2::isInImage(const vpImage &I, const vpImagePoint &ip) */ bool vpDot2::isInArea(const unsigned int &u, const unsigned int &v) const { - unsigned int area_u_min = static_cast(area.getLeft()); - unsigned int area_u_max = static_cast(area.getRight()); - unsigned int area_v_min = static_cast(area.getTop()); - unsigned int area_v_max = static_cast(area.getBottom()); + unsigned int area_u_min = static_cast(m_area.getLeft()); + unsigned int area_u_max = static_cast(m_area.getRight()); + unsigned int area_v_min = static_cast(m_area.getTop()); + unsigned int area_v_max = static_cast(m_area.getBottom()); if ((u < area_u_min) || (u > area_u_max)) { return false; @@ -2228,40 +2323,41 @@ void vpDot2::getGridSize(unsigned int &gridWidth, unsigned int &gridHeight) */ void vpDot2::computeMeanGrayLevel(const vpImage &I) { - int cog_u = static_cast(cog.get_u()); - int cog_v = static_cast(cog.get_v()); + int cog_u = static_cast(m_cog.get_u()); + int cog_v = static_cast(m_cog.get_v()); unsigned int sum_value = 0; unsigned int nb_pixels = 0; - for (unsigned int i = static_cast(this->bbox_u_min); i <= static_cast(this->bbox_u_max); ++i) { + for (unsigned int i = static_cast(m_bbox_u_min); i <= static_cast(m_bbox_u_max); ++i) { unsigned int pixel_gray = static_cast(I[static_cast(cog_v)][i]); if ((pixel_gray >= getGrayLevelMin()) && (pixel_gray <= getGrayLevelMax())) { sum_value += pixel_gray; ++nb_pixels; } } - for (unsigned int i = static_cast(this->bbox_v_min); i <= static_cast(this->bbox_v_max); ++i) { + for (unsigned int i = static_cast(m_bbox_v_min); i <= static_cast(m_bbox_v_max); ++i) { unsigned char pixel_gray = I[i][static_cast(cog_u)]; if ((pixel_gray >= getGrayLevelMin()) && (pixel_gray <= getGrayLevelMax())) { sum_value += pixel_gray; ++nb_pixels; } } - if (nb_pixels < 10) { // could be good to choose the min nb points from area of dot + const unsigned int nb_min_pixels = 10; + if (nb_pixels < nb_min_pixels) { // could be good to choose the min nb points from area of dot // add diagonals points to have enough point int imin, imax; - if ((cog_u - bbox_u_min) >(cog_v - bbox_v_min)) { - imin = cog_v - bbox_v_min; + if ((cog_u - m_bbox_u_min) >(cog_v - m_bbox_v_min)) { + imin = cog_v - m_bbox_v_min; } else { - imin = cog_u - bbox_u_min; + imin = cog_u - m_bbox_u_min; } - if ((bbox_u_max - cog_u) > (bbox_v_max - cog_v)) { - imax = bbox_v_max - cog_v; + if ((m_bbox_u_max - cog_u) > (m_bbox_v_max - cog_v)) { + imax = m_bbox_v_max - cog_v; } else { - imax = bbox_u_max - cog_u; + imax = m_bbox_u_max - cog_u; } for (int i = -imin; i <= imax; ++i) { unsigned int pixel_gray = static_cast(I[static_cast(cog_v + i)][static_cast(cog_u + i)]); @@ -2271,17 +2367,17 @@ void vpDot2::computeMeanGrayLevel(const vpImage &I) } } - if ((cog_u - bbox_u_min) > (bbox_v_max - cog_v)) { - imin = bbox_v_max - cog_v; + if ((cog_u - m_bbox_u_min) > (m_bbox_v_max - cog_v)) { + imin = m_bbox_v_max - cog_v; } else { - imin = cog_u - bbox_u_min; + imin = cog_u - m_bbox_u_min; } - if ((bbox_u_max - cog_u) > (cog_v - bbox_v_min)) { - imax = cog_v - bbox_v_min; + if ((m_bbox_u_max - cog_u) > (cog_v - m_bbox_v_min)) { + imax = cog_v - m_bbox_v_min; } else { - imax = bbox_u_max - cog_u; + imax = m_bbox_u_max - cog_u; } for (int i = -imin; i <= imax; ++i) { @@ -2298,7 +2394,7 @@ void vpDot2::computeMeanGrayLevel(const vpImage &I) throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No point was found")); } else { - mean_gray_level = sum_value / nb_pixels; + m_mean_gray_level = sum_value / nb_pixels; } } @@ -2341,6 +2437,7 @@ vpMatrix vpDot2::defineDots(vpDot2 dot[], const unsigned int &n, const std::stri // read from file and tracks the dots if (fromFile) { try { + const unsigned int cross_size = 10; for (i = 0; i < n; ++i) { cog.set_uv(Cogs[i][0], Cogs[i][1]); dot[i].setGraphics(true); @@ -2348,7 +2445,7 @@ vpMatrix vpDot2::defineDots(vpDot2 dot[], const unsigned int &n, const std::stri if (trackDot) { dot[i].initTracking(I, cog); dot[i].track(I); - vpDisplay::displayCross(I, cog, 10, col); + vpDisplay::displayCross(I, cog, cross_size, col); } } } @@ -2381,6 +2478,7 @@ vpMatrix vpDot2::defineDots(vpDot2 dot[], const unsigned int &n, const std::stri vpDisplay::flush(I); std::cout << "Click on the " << n << " dots clockwise starting from upper/left dot..." << std::endl; + const unsigned int cross_size = 10; for (i = 0; i < n; ++i) { if (trackDot) { dot[i].setGraphics(true); @@ -2393,7 +2491,7 @@ vpMatrix vpDot2::defineDots(vpDot2 dot[], const unsigned int &n, const std::stri } Cogs[i][0] = cog.get_u(); Cogs[i][1] = cog.get_v(); - vpDisplay::displayCross(I, cog, 10, col); + vpDisplay::displayCross(I, cog, cross_size, col); vpDisplay::flush(I); } } @@ -2439,17 +2537,20 @@ void vpDot2::trackAndDisplay(vpDot2 dot[], const unsigned int &n, vpImage &I, const vpImagePoint &cog, const std::list &edges_list, vpColor color, unsigned int thickness) { - vpDisplay::displayCross(I, cog, (3 * thickness) + 8, color, thickness); + const unsigned int val_3 = 3; + const unsigned int val_8 = 8; + + vpDisplay::displayCross(I, cog, (val_3 * thickness) + val_8, color, thickness); std::list::const_iterator it; std::list::const_iterator edges_list_end = edges_list.end(); @@ -2498,7 +2602,9 @@ void vpDot2::display(const vpImage &I, const vpImagePoint &cog, void vpDot2::display(const vpImage &I, const vpImagePoint &cog, const std::list &edges_list, vpColor color, unsigned int thickness) { - vpDisplay::displayCross(I, cog, (3 * thickness) + 8, color, thickness); + const unsigned int val_3 = 3; + const unsigned int val_8 = 8; + vpDisplay::displayCross(I, cog, (val_3 * thickness) + val_8, color, thickness); std::list::const_iterator it; std::list::const_iterator edges_list_end = edges_list.end(); for (it = edges_list.begin(); it != edges_list_end; ++it) { diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbHiddenFaces.h b/modules/tracker/mbt/include/visp3/mbt/vpMbHiddenFaces.h index c034d91265..e9f24bc933 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbHiddenFaces.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbHiddenFaces.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -32,10 +32,11 @@ * in order to have a model based tracker. */ -#ifndef vpMbHiddenFaces_HH -#define vpMbHiddenFaces_HH +#ifndef VP_MB_HIDDEN_FACES_H +#define VP_MB_HIDDEN_FACES_H #include +#include #include #include #include diff --git a/modules/tracker/mbt/src/edge/vpMbtMeLine.cpp b/modules/tracker/mbt/src/edge/vpMbtMeLine.cpp index 23bb3c7cb0..a433359e48 100644 --- a/modules/tracker/mbt/src/edge/vpMbtMeLine.cpp +++ b/modules/tracker/mbt/src/edge/vpMbtMeLine.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,8 @@ * * Description: * Make the complete tracking of an object by using its CAD model - * - * Authors: - * Romain Tallonneau - * -*****************************************************************************/ + */ + #include #ifndef DOXYGEN_SHOULD_SKIP_THIS @@ -46,6 +42,7 @@ #include // std::fabs #include // numeric_limits +#include #include #include #include diff --git a/modules/tracker/me/include/visp3/me/vpMe.h b/modules/tracker/me/include/visp3/me/vpMe.h index ea7e606bd0..dc3d54064d 100644 --- a/modules/tracker/me/include/visp3/me/vpMe.h +++ b/modules/tracker/me/include/visp3/me/vpMe.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,8 +36,8 @@ * \brief Moving edges */ -#ifndef _vpMe_h_ -#define _vpMe_h_ +#ifndef VP_ME_H +#define VP_ME_H #include #include diff --git a/modules/tracker/me/include/visp3/me/vpMeEllipse.h b/modules/tracker/me/include/visp3/me/vpMeEllipse.h index a84b0c418f..9b246c08c7 100644 --- a/modules/tracker/me/include/visp3/me/vpMeEllipse.h +++ b/modules/tracker/me/include/visp3/me/vpMeEllipse.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -33,8 +33,8 @@ * \brief Moving edges on an ellipse */ -#ifndef _vpMeEllipse_h_ -#define _vpMeEllipse_h_ +#ifndef VP_ME_ELLIPSE_H +#define VP_ME_ELLIPSE_H #include #include @@ -134,9 +134,12 @@ class VISP_EXPORT vpMeEllipse : public vpMeTracker inline vpColVector get_nij() const { vpColVector nij(3); - nij[0] = m_n20; - nij[1] = m_n11; - nij[2] = m_n02; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + nij[index_0] = m_n20; + nij[index_1] = m_n11; + nij[index_2] = m_n02; return nij; } @@ -153,9 +156,12 @@ class VISP_EXPORT vpMeEllipse : public vpMeTracker inline vpColVector get_ABE() const { vpColVector ABE(3); - ABE[0] = m_a; - ABE[1] = m_b; - ABE[2] = m_e; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + ABE[index_0] = m_a; + ABE[index_1] = m_b; + ABE[index_2] = m_e; return ABE; } diff --git a/modules/tracker/me/include/visp3/me/vpMeSite.h b/modules/tracker/me/include/visp3/me/vpMeSite.h index e92989c638..93ef098e3f 100644 --- a/modules/tracker/me/include/visp3/me/vpMeSite.h +++ b/modules/tracker/me/include/visp3/me/vpMeSite.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,8 +36,8 @@ * \brief Moving edges */ -#ifndef _vpMeSite_h_ -#define _vpMeSite_h_ +#ifndef VP_ME_SITE_H +#define VP_ME_SITE_H #include #include diff --git a/modules/tracker/me/include/visp3/me/vpMeTracker.h b/modules/tracker/me/include/visp3/me/vpMeTracker.h index c7281b85ed..745a37026b 100644 --- a/modules/tracker/me/include/visp3/me/vpMeTracker.h +++ b/modules/tracker/me/include/visp3/me/vpMeTracker.h @@ -36,8 +36,8 @@ * \brief Contains abstract elements for a Distance to Feature type feature. */ -#ifndef _vpMeTracker_h_ -#define _vpMeTracker_h_ +#ifndef VP_ME_TRACKER_H +#define VP_ME_TRACKER_H #include #include diff --git a/modules/tracker/me/src/moving-edges/vpMe.cpp b/modules/tracker/me/src/moving-edges/vpMe.cpp index e4d32d4773..f9c54129d6 100644 --- a/modules/tracker/me/src/moving-edges/vpMe.cpp +++ b/modules/tracker/me/src/moving-edges/vpMe.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -59,6 +59,14 @@ struct vpDroite2Dt double c; }; +struct vpBBoxt +{ + double Xmin; + double Ymin; + double Xmax; + double Ymax; +}; + template inline void permute(Type &a, Type &b) { Type t = a; @@ -89,22 +97,22 @@ static vpPoint2Dt pointIntersection(vpDroite2Dt D1, vpDroite2Dt D2) return I; } -static void recale(vpPoint2Dt &P, double Xmin, double Ymin, double Xmax, double Ymax) +static void recale(vpPoint2Dt &P, const vpBBoxt &bbox) { - if (vpMath::equal(P.x, Xmin)) { - P.x = Xmin; // a peu pres => exactement ! + if (vpMath::equal(P.x, bbox.Xmin)) { + P.x = bbox.Xmin; // a peu pres => exactement ! } - if (vpMath::equal(P.x, Xmax)) { - P.x = Xmax; + if (vpMath::equal(P.x, bbox.Xmax)) { + P.x = bbox.Xmax; } - if (vpMath::equal(P.y, Ymin)) { - P.y = Ymin; + if (vpMath::equal(P.y, bbox.Ymin)) { + P.y = bbox.Ymin; } - if (vpMath::equal(P.y, Ymax)) { - P.y = Ymax; + if (vpMath::equal(P.y, bbox.Ymax)) { + P.y = bbox.Ymax; } } @@ -121,27 +129,36 @@ static void permute(vpPoint2Dt &A, vpPoint2Dt &B) } // vrai si partie visible -static bool clipping(vpPoint2Dt A, vpPoint2Dt B, double Xmin, double Ymin, double Xmax, double Ymax, vpPoint2Dt &Ac, +static bool clipping(vpPoint2Dt A, vpPoint2Dt B, const vpBBoxt &bbox, vpPoint2Dt &Ac, vpPoint2Dt &Bc) // resultat: A,B clippes { vpDroite2Dt AB, D[4]; - D[0].a = 1; - D[0].b = 0; - D[0].c = -Xmin; - D[1].a = 1; - D[1].b = 0; - D[1].c = -Xmax; - D[2].a = 0; - D[2].b = 1; - D[2].c = -Ymin; - D[3].a = 0; - D[3].b = 1; - D[3].c = -Ymax; - - const int nbP = 2; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int val_1 = 1; + const unsigned int val_2 = 2; + const unsigned int val_4 = 4; + const unsigned int val_8 = 8; + + D[index_0].a = 1; + D[index_0].b = 0; + D[index_0].c = -bbox.Xmin; + D[index_1].a = 1; + D[index_1].b = 0; + D[index_1].c = -bbox.Xmax; + D[index_2].a = 0; + D[index_2].b = 1; + D[index_2].c = -bbox.Ymin; + D[index_3].a = 0; + D[index_3].b = 1; + D[index_3].c = -bbox.Ymax; + + const int nbP = val_2; vpPoint2Dt P[nbP]; - P[0] = A; - P[1] = B; + P[index_0] = A; + P[index_1] = B; unsigned int code_P[nbP], // codes de P[n] i, bit_i, // i -> (0000100...) n; @@ -155,20 +172,20 @@ static bool clipping(vpPoint2Dt A, vpPoint2Dt B, double Xmin, double Ymin, doubl for (n = 0; n < nbP; ++n) { code_P[n] = 0; - if (P[n].x < Xmin) { - code_P[n] |= 1; // positionne bit0 + if (P[n].x < bbox.Xmin) { + code_P[n] |= val_1; // positionne bit0 } - if (P[n].x > Xmax) { - code_P[n] |= 2; // .. bit1 + if (P[n].x > bbox.Xmax) { + code_P[n] |= val_2; // .. bit1 } - if (P[n].y < Ymin) { - code_P[n] |= 4; // .. bit2 + if (P[n].y < bbox.Ymin) { + code_P[n] |= val_4; // .. bit2 } - if (P[n].y > Ymax) { - code_P[n] |= 8; // .. bit3 + if (P[n].y > bbox.Ymax) { + code_P[n] |= val_8; // .. bit3 } } @@ -218,76 +235,77 @@ static bool clipping(vpPoint2Dt A, vpPoint2Dt B, double Xmin, double Ymin, doubl // RECALE EXACTEMENT LE POINT (calcul flottant => arrondi) // AFIN QUE LE CALCUL DES CODES NE BOUCLE PAS INDEFINIMENT // ======================================================= - recale(P[n], Xmin, Ymin, Xmax, Ymax); + recale(P[n], bbox); } } // calcule la surface relative des 2 portions definies // par le segment PQ sur le carre Xmin,Ymin,Xmax,Ymax // Rem : P,Q tries sur x, et donc seulement 6 cas -static double surfaceRelative(vpPoint2Dt P, vpPoint2Dt Q, double Xmin, double Ymin, double Xmax, double Ymax) +static double surfaceRelative(vpPoint2Dt P, vpPoint2Dt Q, const vpBBoxt &bbox) { if (Q.x < P.x) { // tri le couple de points permute(P, Q); // selon leur abscisse x } - recale(P, Xmin, Ymin, Xmax, Ymax); // permet des calculs de S_relative - recale(Q, Xmin, Ymin, Xmax, Ymax); // moins approximatifs. + recale(P, bbox); // permet des calculs de S_relative + recale(Q, bbox); // moins approximatifs. // Case P.x=Xmin and Q.x=Xmax - if ((std::fabs(P.x - Xmin) <= - (vpMath::maximum(std::fabs(P.x), std::fabs(Xmin)) * std::numeric_limits::epsilon())) && - (std::fabs(Q.x - Xmax) <= - (vpMath::maximum(std::fabs(Q.x), std::fabs(Xmax)) * std::numeric_limits::epsilon()))) { - return fabs((Ymax + Ymin) - (P.y - Q.y)); + if ((std::fabs(P.x - bbox.Xmin) <= + (vpMath::maximum(std::fabs(P.x), std::fabs(bbox.Xmin)) * std::numeric_limits::epsilon())) && + (std::fabs(Q.x - bbox.Xmax) <= + (vpMath::maximum(std::fabs(Q.x), std::fabs(bbox.Xmax)) * std::numeric_limits::epsilon()))) { + return fabs((bbox.Ymax + bbox.Ymin) - (P.y - Q.y)); } // Case (P.y=Ymin and Q.y==Ymax) or (Q.y=Ymin and P.y==Ymax) - if (((std::fabs(P.y - Ymin) <= - (vpMath::maximum(std::fabs(P.y), std::fabs(Ymin)) * std::numeric_limits::epsilon())) && - (std::fabs(Q.y - Ymax) <= - (vpMath::maximum(std::fabs(Q.y), std::fabs(Ymax)) * std::numeric_limits::epsilon()))) || - ((std::fabs(Q.y - Ymin) <= - (vpMath::maximum(std::fabs(Q.y), std::fabs(Ymin)) * std::numeric_limits::epsilon())) && - (std::fabs(P.y - Ymax) <= - (vpMath::maximum(std::fabs(P.y), std::fabs(Ymax)) * std::numeric_limits::epsilon())))) { - return fabs((Xmax + Xmin) - (P.x - Q.x)); + if (((std::fabs(P.y - bbox.Ymin) <= + (vpMath::maximum(std::fabs(P.y), std::fabs(bbox.Ymin)) * std::numeric_limits::epsilon())) && + (std::fabs(Q.y - bbox.Ymax) <= + (vpMath::maximum(std::fabs(Q.y), std::fabs(bbox.Ymax)) * std::numeric_limits::epsilon()))) || + ((std::fabs(Q.y - bbox.Ymin) <= + (vpMath::maximum(std::fabs(Q.y), std::fabs(bbox.Ymin)) * std::numeric_limits::epsilon())) && + (std::fabs(P.y - bbox.Ymax) <= + (vpMath::maximum(std::fabs(P.y), std::fabs(bbox.Ymax)) * std::numeric_limits::epsilon())))) { + return fabs((bbox.Xmax + bbox.Xmin) - (P.x - Q.x)); } // Case P.x=Xmin and Q.y=Ymax - if ((std::fabs(P.x - Xmin) <= - (vpMath::maximum(std::fabs(P.x), std::fabs(Xmin)) * std::numeric_limits::epsilon())) && - (std::fabs(Q.y - Ymax) <= - (vpMath::maximum(std::fabs(Q.y), std::fabs(Ymax)) * std::numeric_limits::epsilon()))) { - return (1 - ((Ymax - P.y) * (Q.x - Xmin))); + if ((std::fabs(P.x - bbox.Xmin) <= + (vpMath::maximum(std::fabs(P.x), std::fabs(bbox.Xmin)) * std::numeric_limits::epsilon())) && + (std::fabs(Q.y - bbox.Ymax) <= + (vpMath::maximum(std::fabs(Q.y), std::fabs(bbox.Ymax)) * std::numeric_limits::epsilon()))) { + return (1 - ((bbox.Ymax - P.y) * (Q.x - bbox.Xmin))); } // Case P.x=Xmin and Q.y=Ymin - if ((std::fabs(P.x - Xmin) <= - (vpMath::maximum(std::fabs(P.x), std::fabs(Xmin)) * std::numeric_limits::epsilon())) && - (std::fabs(Q.y - Ymin) <= - (vpMath::maximum(std::fabs(Q.y), std::fabs(Ymin)) * std::numeric_limits::epsilon()))) { - return (1 - ((P.y - Ymin) * (Q.x - Xmin))); + if ((std::fabs(P.x - bbox.Xmin) <= + (vpMath::maximum(std::fabs(P.x), std::fabs(bbox.Xmin)) * std::numeric_limits::epsilon())) && + (std::fabs(Q.y - bbox.Ymin) <= + (vpMath::maximum(std::fabs(Q.y), std::fabs(bbox.Ymin)) * std::numeric_limits::epsilon()))) { + return (1 - ((P.y - bbox.Ymin) * (Q.x - bbox.Xmin))); } // Case P.y=Ymin and Q.x=Xmax - if ((std::fabs(P.y - Ymin) <= - (vpMath::maximum(std::fabs(P.y), std::fabs(Ymin)) * std::numeric_limits::epsilon())) && - (std::fabs(Q.x - Xmax) <= - (vpMath::maximum(std::fabs(Q.x), std::fabs(Xmax)) * std::numeric_limits::epsilon()))) { - return (1 - ((Xmax - P.x) * (Q.y - Ymin))); + if ((std::fabs(P.y - bbox.Ymin) <= + (vpMath::maximum(std::fabs(P.y), std::fabs(bbox.Ymin)) * std::numeric_limits::epsilon())) && + (std::fabs(Q.x - bbox.Xmax) <= + (vpMath::maximum(std::fabs(Q.x), std::fabs(bbox.Xmax)) * std::numeric_limits::epsilon()))) { + return (1 - ((bbox.Xmax - P.x) * (Q.y - bbox.Ymin))); } // Case P.y=Ymax and Q.x=Xmax - if ((std::fabs(P.y - Ymax) <= - (vpMath::maximum(std::fabs(P.y), std::fabs(Ymax)) * std::numeric_limits::epsilon())) && - (std::fabs(Q.x - Xmax) <= - (vpMath::maximum(std::fabs(Q.x), std::fabs(Xmax)) * std::numeric_limits::epsilon()))) { - return (1 - ((Xmax - P.x) * (Ymax - Q.y))); + if ((std::fabs(P.y - bbox.Ymax) <= + (vpMath::maximum(std::fabs(P.y), std::fabs(bbox.Ymax)) * std::numeric_limits::epsilon())) && + (std::fabs(Q.x - bbox.Xmax) <= + (vpMath::maximum(std::fabs(Q.x), std::fabs(bbox.Xmax)) * std::numeric_limits::epsilon()))) { + return (1 - ((bbox.Xmax - P.x) * (bbox.Ymax - Q.y))); } - throw(vpException(vpException::fatalError, "utils_ecm: error in surfaceRelative (%f,%f) (%f,%f) %f %f %f %f", P.x, P.y, Q.x, Q.y, Xmin, Ymin, Xmax, Ymax)); + throw(vpException(vpException::fatalError, "utils_ecm: error in surfaceRelative (%f,%f) (%f,%f) %f %f %f %f", + P.x, P.y, Q.x, Q.y, bbox.Xmin, bbox.Ymin, bbox.Xmax, bbox.Ymax)); } static void calculMasques(vpColVector &angle, // definitions des angles theta @@ -341,9 +359,14 @@ static void calculMasques(vpColVector &angle, // definitions des angles theta int sgn = vpMath::sign((cos_theta * Y) - (sin_theta * X)); // Resultat = P,Q - if (clipping(P1, Q1, X - 0.5, Y - 0.5, X + 0.5, Y + 0.5, P, Q)) { + vpBBoxt bbox; + bbox.Xmin = X - 0.5; + bbox.Ymin = Y - 0.5; + bbox.Xmax = X + 0.5; + bbox.Ymax = Y + 0.5; + if (clipping(P1, Q1, bbox, P, Q)) { // v dans [0,1] - v = surfaceRelative(P, Q, X - 0.5, Y - 0.5, X + 0.5, Y + 0.5); + v = surfaceRelative(P, Q, bbox); } else { v = 1; // PQ ne coupe pas le pixel(i,j) @@ -399,28 +422,63 @@ void vpMe::print() std::cout << " Minimum likelihood threshold....." << "unused" << std::endl; } - std::cout << " Contrast tolerance +/-..........." << m_mu1 * 100 << "% and " << m_mu2 * 100 << "% " << std::endl; + const unsigned int val_100 = 100; + std::cout << " Contrast tolerance +/-..........." << (m_mu1 * val_100) << "% and " << (m_mu2 * val_100) << "% " << std::endl; std::cout << " Sample step......................" << m_sample_step << " pixels" << std::endl; std::cout << " Strip............................" << m_strip << " pixels " << std::endl; std::cout << " Min sample step.................." << m_min_samplestep << " pixels " << std::endl; } vpMe::vpMe() - : m_likelihood_threshold_type(OLD_THRESHOLD), m_threshold(10000), m_thresholdMarginRatio(-1), m_minThreshold(-1), m_useAutomaticThreshold(false), - m_mu1(0.5), m_mu2(0.5), m_min_samplestep(4), m_anglestep(1), m_mask_sign(0), m_range(4), m_sample_step(10), - m_ntotal_sample(0), m_points_to_track(500), m_mask_size(5), m_mask_number(180), m_strip(2), m_mask(nullptr) + : m_likelihood_threshold_type(OLD_THRESHOLD), m_thresholdMarginRatio(-1), m_minThreshold(-1), m_useAutomaticThreshold(false), + m_mu1(0.5), m_mu2(0.5), m_anglestep(1), m_mask_sign(0), m_ntotal_sample(0), m_mask(nullptr) { + const double threshold_default = 1000; + const int points_to_track_default = 500; + const unsigned int mask_number_default = 180; + const unsigned int mask_size_default = 5; + const int strip_default = 2; + const double min_samplestep_default = 4; const unsigned int flatAngle = 180; + const unsigned int range_default = 4; + const double sample_step_default = 10; + + m_threshold = threshold_default; + m_points_to_track = points_to_track_default; + m_mask_number = mask_number_default; + m_mask_size = mask_size_default; + m_strip = strip_default; + m_min_samplestep = min_samplestep_default; + m_range = range_default; + m_sample_step = sample_step_default; + m_anglestep = (flatAngle / m_mask_number); initMask(); } vpMe::vpMe(const vpMe &me) - : m_likelihood_threshold_type(OLD_THRESHOLD), m_threshold(10000), m_thresholdMarginRatio(-1), m_minThreshold(-1), m_useAutomaticThreshold(false), - m_mu1(0.5), m_mu2(0.5), m_min_samplestep(4), m_anglestep(1), m_mask_sign(0), m_range(4), m_sample_step(10), - m_ntotal_sample(0), m_points_to_track(500), m_mask_size(5), m_mask_number(180), m_strip(2), m_mask(nullptr) + : m_likelihood_threshold_type(OLD_THRESHOLD), m_thresholdMarginRatio(-1), m_minThreshold(-1), m_useAutomaticThreshold(false), + m_mu1(0.5), m_mu2(0.5), m_anglestep(1), m_mask_sign(0), m_ntotal_sample(0), m_mask(nullptr) { + const double threshold_default = 1000; + const int points_to_track_default = 500; + const unsigned int mask_number_default = 180; + const unsigned int mask_size_default = 5; + const int strip_default = 2; + const double min_samplestep_default = 4; + const unsigned int range_default = 4; + const double sample_step_default = 10; + + m_threshold = threshold_default; + m_points_to_track = points_to_track_default; + m_mask_number = mask_number_default; + m_mask_size = mask_size_default; + m_strip = strip_default; + m_min_samplestep = min_samplestep_default; + m_range = range_default; + m_sample_step = sample_step_default; + *this = me; } diff --git a/modules/tracker/me/src/moving-edges/vpMeEllipse.cpp b/modules/tracker/me/src/moving-edges/vpMeEllipse.cpp index a9bc841e99..25c922de56 100644 --- a/modules/tracker/me/src/moving-edges/vpMeEllipse.cpp +++ b/modules/tracker/me/src/moving-edges/vpMeEllipse.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -32,7 +32,6 @@ #include // numeric_limits #include -#include #include #include #include @@ -40,21 +39,23 @@ #include #include -BEGIN_VISP_NAMESPACE - -// --comment: define the VP_ME_ELLIPSE_REGULAR_SAMPLING flag #ifndef VP_ME_ELLIPSE_REGULAR_SAMPLING #define VP_ME_ELLIPSE_TWO_CONCENTRIC_CIRCLES #endif +BEGIN_VISP_NAMESPACE + vpMeEllipse::vpMeEllipse() - : m_K(), m_iPc(), m_a(0.), m_b(0.), m_e(0.), m_iP1(), m_iP2(), m_alpha1(0), m_alpha2(2 * M_PI), m_ce(0.), m_se(0.), m_angleList(), m_m00(0.), + : m_K(), m_iPc(), m_a(0.), m_b(0.), m_e(0.), m_iP1(), m_iP2(), m_alpha1(0), m_ce(0.), m_se(0.), m_angleList(), m_m00(0.), m_thresholdWeight(0.2), m_alphamin(0.), m_alphamax(0.), m_uc(0.), m_vc(0.), m_n20(0.), m_n11(0.), m_n02(0.), m_expectedDensity(0), m_numberOfGoodPoints(0), m_trackCircle(false), m_trackArc(false), m_arcEpsilon(1e-6) { + const unsigned int val_2 = 2; + const unsigned int val_6 = 6; + m_alpha2 = val_2 * M_PI; // Resize internal parameters vector // K0 u^2 + K1 v^2 + 2 K2 u v + 2 K3 u + 2 K4 v + K5 = 0 - m_K.resize(6); + m_K.resize(val_6); m_iP1.set_ij(0, 0); m_iP2.set_ij(0, 0); } @@ -208,12 +209,18 @@ void vpMeEllipse::computeAbeFromNij() void vpMeEllipse::computeKiFromNij() { - m_K[0] = m_n02; - m_K[1] = m_n20; - m_K[2] = -m_n11; - m_K[3] = (m_n11 * m_vc) - (m_n02 * m_uc); - m_K[4] = (m_n11 * m_uc) - (m_n20 * m_vc); - m_K[5] = (((m_n02 * m_uc * m_uc) + (m_n20 * m_vc * m_vc)) - (2.0 * m_n11 * m_uc * m_vc)) + (4.0 * ((m_n11 * m_n11) - (m_n20 * m_n02))); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + const unsigned int index_5 = 5; + m_K[index_0] = m_n02; + m_K[index_1] = m_n20; + m_K[index_2] = -m_n11; + m_K[index_3] = (m_n11 * m_vc) - (m_n02 * m_uc); + m_K[index_4] = (m_n11 * m_uc) - (m_n20 * m_vc); + m_K[index_5] = (((m_n02 * m_uc * m_uc) + (m_n20 * m_vc * m_vc)) - (2.0 * m_n11 * m_uc * m_vc)) + (4.0 * ((m_n11 * m_n11) - (m_n20 * m_n02))); } void vpMeEllipse::computeNijFromAbe() @@ -225,32 +232,35 @@ void vpMeEllipse::computeNijFromAbe() void vpMeEllipse::getParameters() { + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + const unsigned int index_5 = 5; // Equations below from Chaumette PhD and TRO 2004 paper - double num = (m_K[0] * m_K[1]) - (m_K[2] * m_K[2]); // > 0 for an ellipse + double num = (m_K[index_0] * m_K[index_1]) - (m_K[index_2] * m_K[index_2]); // > 0 for an ellipse if (num <= 0) { throw(vpTrackingException(vpTrackingException::fatalError, "The points do not belong to an ellipse! num: %f", num)); } - m_uc = ((m_K[2] * m_K[4]) - (m_K[1] * m_K[3])) / num; - m_vc = ((m_K[2] * m_K[3]) - (m_K[0] * m_K[4])) / num; + m_uc = ((m_K[index_2] * m_K[index_4]) - (m_K[index_1] * m_K[index_3])) / num; + m_vc = ((m_K[index_2] * m_K[index_3]) - (m_K[index_0] * m_K[index_4])) / num; m_iPc.set_uv(m_uc, m_vc); - double d = (((m_K[0] * m_uc * m_uc) + (m_K[1] * m_vc * m_vc) + (2.0 * m_K[2] * m_uc * m_vc)) - m_K[5]) / (4.0 * num); - m_n20 = m_K[1] * d; // always > 0 - m_n11 = -m_K[2] * d; - m_n02 = m_K[0] * d; // always > 0 + double d = (((m_K[index_0] * m_uc * m_uc) + (m_K[index_1] * m_vc * m_vc) + (2.0 * m_K[index_2] * m_uc * m_vc)) - m_K[index_5]) / (4.0 * num); + m_n20 = m_K[index_1] * d; // always > 0 + m_n11 = -m_K[index_2] * d; + m_n02 = m_K[index_0] * d; // always > 0 computeAbeFromNij(); // normalization so that K0 = n02, K1 = n20, etc (Eq (25) of TRO paper) - d = m_n02 / m_K[0]; // fabs(K[0]) > 0 + d = m_n02 / m_K[index_0]; // fabs(K[0]) > 0 unsigned int Ksize = m_K.size(); for (unsigned int i = 0; i < Ksize; ++i) { m_K[i] *= d; } - if (vpDEBUG_ENABLE(3)) { - printParameters(); - } } void vpMeEllipse::printParameters() const @@ -338,9 +348,10 @@ unsigned int vpMeEllipse::plugHoles(const vpImage &I) unsigned int nb_pts_added = 0; int nbrows = static_cast(I.getHeight()); int nbcols = static_cast(I.getWidth()); + const unsigned int range_default = 2; unsigned int memory_range = m_me->getRange(); - m_me->setRange(2); + m_me->setRange(range_default); // Perimeter of the ellipse using Ramanujan formula double perim = M_PI * ((3.0 * (m_a + m_b)) - sqrt(((3.0 * m_a) + m_b) * (m_a + (3.0 * m_b)))); @@ -389,10 +400,6 @@ unsigned int vpMeEllipse::plugHoles(const vpImage &I) } m_meList.insert(meList, pix); m_angleList.insert(angleList, new_ang); - if (vpDEBUG_ENABLE(3)) { - const unsigned int crossSize = 5; - vpDisplay::displayCross(I, iP, crossSize, vpColor::blue); - } } } ang += incr; @@ -403,12 +410,6 @@ unsigned int vpMeEllipse::plugHoles(const vpImage &I) ++meList; } - if (vpDEBUG_ENABLE(3)) { - if (nb_pts_added > 0) { - std::cout << "Number of added points from holes with angles: " << nb_pts_added << std::endl; - } - } - // Add points in case two neighboring points are too far away angleList = m_angleList.begin(); ang = *angleList; @@ -450,10 +451,6 @@ unsigned int vpMeEllipse::plugHoles(const vpImage &I) } m_meList.insert(meList, pix); m_angleList.insert(angleList, new_ang); - if (vpDEBUG_ENABLE(3)) { - const unsigned int crossSize = 5; - vpDisplay::displayCross(I, iP, crossSize, vpColor::blue); - } } } } @@ -463,18 +460,6 @@ unsigned int vpMeEllipse::plugHoles(const vpImage &I) ++meList; } - if (vpDEBUG_ENABLE(3)) { - if (nb_pts_added > 0) { - std::cout << "Number of added points from holes : " << nb_pts_added << std::endl; - angleList = m_angleList.begin(); - while (angleList != m_angleList.end()) { - ang = *angleList; - std::cout << "ang = " << vpMath::deg(ang) << std::endl; - ++angleList; - } - } - } - // Try to fill the first extremity: from alpha_min - incr to alpha1 + incr/2 meList = m_meList.begin(); pix1 = *meList; @@ -512,22 +497,11 @@ unsigned int vpMeEllipse::plugHoles(const vpImage &I) } m_meList.push_front(pix); m_angleList.push_front(new_ang); - if (vpDEBUG_ENABLE(3)) { - const unsigned int crossSize = 5; - vpDisplay::displayCross(I, iP, crossSize, vpColor::blue); - std::cout << "Add extremity 1, ang = " << vpMath::deg(new_ang) << std::endl; - } } } ang -= incr; } - if (vpDEBUG_ENABLE(3)) { - if (nb_pts_added > 0) { - std::cout << "Number of added points from holes and first extremity : " << nb_pts_added << std::endl; - } - } - // Try to fill the second extremity: from alphamax + incr to alpha2 - incr/2 pix1 = m_meList.back(); nbpts = 0; @@ -563,11 +537,6 @@ unsigned int vpMeEllipse::plugHoles(const vpImage &I) } m_meList.push_back(pix); m_angleList.push_back(new_ang); - if (vpDEBUG_ENABLE(3)) { - const unsigned int crossSize = 5; - vpDisplay::displayCross(I, iP, crossSize, vpColor::blue); - std::cout << "Add extremity 2, ang = " << vpMath::deg(new_ang) << std::endl; - } } } ang += incr; @@ -580,11 +549,6 @@ unsigned int vpMeEllipse::plugHoles(const vpImage &I) m_meList.size(), m_angleList.size())); } - if (vpDEBUG_ENABLE(3)) { - if (nb_pts_added > 0) { - std::cout << "In plugHoles(): nb of added points : " << nb_pts_added << std::endl; - } - } return nb_pts_added; } @@ -593,6 +557,12 @@ void vpMeEllipse::leastSquare(const vpImage &I, const std::vector double um = I.getWidth() / 2.; double vm = I.getHeight() / 2.; unsigned int n = static_cast(iP.size()); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + const unsigned int index_5 = 5; if (m_trackCircle) { // we track a circle const unsigned int circleDims = 3; @@ -609,9 +579,9 @@ void vpMeEllipse::leastSquare(const vpImage &I, const std::vector // normalization so that (u,v) in [-1;1] double u = (iP[k].get_u() - um) / um; double v = (iP[k].get_v() - vm) / um; // um here to not deform the circle - A[k][0] = u; - A[k][1] = v; - A[k][2] = 1.0; + A[k][index_0] = u; + A[k][index_1] = v; + A[k][index_2] = 1.0; b[k] = (u * u) + (v * v); } vpColVector x(3); @@ -619,14 +589,15 @@ void vpMeEllipse::leastSquare(const vpImage &I, const std::vector // A circle is a particular ellipse. Going from x for circle to K for ellipse // using inverse normalization to go back to pixel values double ratio = vm / um; - m_K[0] = (m_K[1] = (1.0 / (um * um))); - m_K[2] = 0.0; - m_K[3] = -(1.0 + (x[0] / 2.0)) / um; - m_K[4] = -(ratio + (x[1] / 2.0)) / um; - m_K[5] = -x[2] + 1.0 + (ratio * ratio) + x[0] + (ratio * x[1]); + m_K[index_0] = (m_K[index_1] = (1.0 / (um * um))); + m_K[index_2] = 0.0; + m_K[index_3] = -(1.0 + (x[index_0] / 2.0)) / um; + m_K[index_4] = -(ratio + (x[index_1] / 2.0)) / um; + m_K[index_5] = -x[index_2] + 1.0 + (ratio * ratio) + x[index_0] + (ratio * x[index_1]); } else { // we track an ellipse - if (n < 5) { + const unsigned int npoints_min = 5; + if (n < npoints_min) { throw(vpException(vpException::dimensionError, "Not enough points to compute the ellipse")); } // Homogeneous system A x = 0 ; x is the nullspace of A @@ -646,12 +617,12 @@ void vpMeEllipse::leastSquare(const vpImage &I, const std::vector // Normalization so that (u,v) in [-1;1] double u = (iP[k].get_u() - um) / um; double v = (iP[k].get_v() - vm) / vm; - A[k][0] = u * u; - A[k][1] = v * v; - A[k][2] = 2.0 * u * v; - A[k][3] = 2.0 * u; - A[k][4] = 2.0 * v; - A[k][5] = 1.0; + A[k][index_0] = u * u; + A[k][index_1] = v * v; + A[k][index_2] = 2.0 * u * v; + A[k][index_3] = 2.0 * u; + A[k][index_4] = 2.0 * v; + A[k][index_5] = 1.0; } vpMatrix KerA; unsigned int dim = A.nullSpace(KerA, 1); @@ -664,11 +635,12 @@ void vpMeEllipse::leastSquare(const vpImage &I, const std::vector } // inverse normalization - m_K[0] *= vm / um; - m_K[1] *= um / vm; - m_K[3] = (m_K[3] * vm) - (m_K[0] * um) - (m_K[2] * vm); - m_K[4] = (m_K[4] * um) - (m_K[1] * vm) - (m_K[2] * um); - m_K[5] = (m_K[5] * um * vm) - (m_K[0] * um * um) - (m_K[1] * vm * vm) - (2.0 * m_K[2] * um * vm) - (2.0 * m_K[3] * um) - (2.0 * m_K[4] * vm); + m_K[index_0] *= vm / um; + m_K[index_1] *= um / vm; + m_K[index_3] = (m_K[index_3] * vm) - (m_K[index_0] * um) - (m_K[index_2] * vm); + m_K[index_4] = (m_K[index_4] * um) - (m_K[index_1] * vm) - (m_K[index_2] * um); + m_K[index_5] = (m_K[index_5] * um * vm) - (m_K[index_0] * um * um) - (m_K[index_1] * vm * vm) - + (2.0 * m_K[index_2] * um * vm) - (2.0 * m_K[index_3] * um) - (2.0 * m_K[index_4] * vm); } getParameters(); } @@ -680,6 +652,12 @@ unsigned int vpMeEllipse::leastSquareRobust(const vpImage &I) const unsigned int nos = numberOfSignal(); unsigned int k = 0; // count the number of tracked MEs + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + const unsigned int index_5 = 5; vpColVector w(nos); w = 1.0; @@ -704,9 +682,9 @@ unsigned int vpMeEllipse::leastSquareRobust(const vpImage &I) // from (i,j) to (u,v) frame + normalization so that (u,v) in [-1;1] double u = (p_me.get_jfloat() - um) / um; double v = (p_me.get_ifloat() - vm) / um; // um to not deform the circle - A[k][0] = u; - A[k][1] = v; - A[k][2] = 1.0; + A[k][index_0] = u; + A[k][index_1] = v; + A[k][index_2] = 1.0; b[k] = (u * u) + (v * v); // Useful to compute the weights in the robust estimation xp[k] = p_me.get_jfloat(); @@ -748,11 +726,11 @@ unsigned int vpMeEllipse::leastSquareRobust(const vpImage &I) // A circle is a particular ellipse. Going from x for circle to K for ellipse // using inverse normalization to go back to pixel values double ratio = vm / um; - m_K[0] = (m_K[1] = (1.0 / (um * um))); - m_K[2] = 0.0; - m_K[3] = -(1.0 + (x[0] / 2.0)) / um; - m_K[4] = -(ratio + (x[1] / 2.0)) / um; - m_K[5] = -x[2] + 1.0 + (ratio * ratio) + x[0] + (ratio * x[1]); + m_K[index_0] = (m_K[index_1] = (1.0 / (um * um))); + m_K[index_2] = 0.0; + m_K[index_3] = -(1.0 + (x[index_0] / 2.0)) / um; + m_K[index_4] = -(ratio + (x[index_1] / 2.0)) / um; + m_K[index_5] = -x[index_2] + 1.0 + (ratio * ratio) + x[index_0] + (ratio * x[index_1]); getParameters(); vpColVector xg(2); @@ -765,7 +743,8 @@ unsigned int vpMeEllipse::leastSquareRobust(const vpImage &I) for (unsigned int i = 0; i < k; ++i) { double x = xp[i]; double y = yp[i]; - double sign = (m_K[0] * x * x) + (m_K[1] * y * y) + (2. * m_K[2] * x * y) + (2. * m_K[3] * x) + (2. * m_K[4] * y) + m_K[5]; + double sign = (m_K[index_0] * x * x) + (m_K[index_1] * y * y) + (2. * m_K[index_2] * x * y) + + (2. * m_K[index_3] * x) + (2. * m_K[index_4] * y) + m_K[index_5]; vpImagePoint ip1, ip2; ip1.set_uv(x, y); double ang = computeAngleOnEllipse(ip1); @@ -807,12 +786,12 @@ unsigned int vpMeEllipse::leastSquareRobust(const vpImage &I) // from (i,j) to (u,v) frame + normalization so that (u,v) in [-1;1] double u = (p_me.get_jfloat() - um) / um; double v = (p_me.get_ifloat() - vm) / vm; - A[k][0] = u * u; - A[k][1] = v * v; - A[k][2] = 2.0 * u * v; - A[k][3] = 2.0 * u; - A[k][4] = 2.0 * v; - A[k][5] = 1.0; + A[k][index_0] = u * u; + A[k][index_1] = v * v; + A[k][index_2] = 2.0 * u * v; + A[k][index_3] = 2.0 * u; + A[k][index_4] = 2.0 * v; + A[k][index_5] = 1.0; // Useful to compute the weights in the robust estimation xp[k] = p_me.get_jfloat(); yp[k] = p_me.get_ifloat(); @@ -850,17 +829,18 @@ unsigned int vpMeEllipse::leastSquareRobust(const vpImage &I) throw(vpMatrixException(vpMatrixException::rankDeficient, "Linear system for computing the ellipse equation ill conditioned")); } - - for (unsigned int i = 0; i < 6; ++i) { + const unsigned int nparam = 6; + for (unsigned int i = 0; i < nparam; ++i) { m_K[i] = KerDA[i][0]; // norm(K) = 1 } // inverse normalization - m_K[0] *= vm / um; - m_K[1] *= um / vm; - m_K[3] = (m_K[3] * vm) - (m_K[0] * um) - (m_K[2] * vm); - m_K[4] = (m_K[4] * um) - (m_K[1] * vm) - (m_K[2] * um); - m_K[5] = (m_K[5] * um * vm) - (m_K[0] * um * um) - (m_K[1] * vm * vm) - (2.0 * m_K[2] * um * vm) - (2.0 * m_K[3] * um) - (2.0 * m_K[4] * vm); + m_K[index_0] *= vm / um; + m_K[index_1] *= um / vm; + m_K[index_3] = (m_K[index_3] * vm) - (m_K[0] * um) - (m_K[index_2] * vm); + m_K[index_4] = (m_K[index_4] * um) - (m_K[1] * vm) - (m_K[index_2] * um); + m_K[index_5] = (m_K[index_5] * um * vm) - (m_K[index_0] * um * um) - (m_K[index_1] * vm * vm) + - (2.0 * m_K[index_2] * um * vm) - (2.0 * m_K[index_3] * um) - (2.0 * m_K[index_4] * vm); getParameters(); // since a, b, and e are used just after vpColVector xg(2); @@ -903,30 +883,13 @@ unsigned int vpMeEllipse::leastSquareRobust(const vpImage &I) vpMeSite p_me = *meList; if (p_me.getState() != vpMeSite::NO_SUPPRESSION) { // points not selected as me - double ang = *angleList; meList = m_meList.erase(meList); angleList = m_angleList.erase(angleList); - if (vpDEBUG_ENABLE(3)) { - vpImagePoint iP; - iP.set_ij(p_me.m_ifloat, p_me.m_jfloat); - printf("point %d not me i : %.0f , j : %0.f, ang = %lf\n", k, p_me.get_ifloat(), p_me.get_jfloat(), vpMath::deg(ang)); - const unsigned int crossSize = 10; - vpDisplay::displayCross(I, iP, crossSize, vpColor::blue, 1); - } } else { if (w[k] < m_thresholdWeight) { // outlier - double ang = *angleList; meList = m_meList.erase(meList); angleList = m_angleList.erase(angleList); - if (vpDEBUG_ENABLE(3)) { - vpImagePoint iP; - iP.set_ij(p_me.m_ifloat, p_me.m_jfloat); - printf("point %d outlier i : %.0f , j : %0.f, ang = %lf, poids : %lf\n", - k, p_me.get_ifloat(), p_me.get_jfloat(), vpMath::deg(ang), w[k]); - const unsigned int crossSize = 10; - vpDisplay::displayCross(I, iP, crossSize, vpColor::cyan, 1); - } } else { // good point double ang = *angleList; @@ -943,11 +906,6 @@ unsigned int vpMeEllipse::leastSquareRobust(const vpImage &I) *angleList = new_ang; ++meList; ++angleList; - if (vpDEBUG_ENABLE(3)) { - printf("point %d inlier i : %.0f , j : %0.f, poids : %lf\n", k, p_me.get_ifloat(), p_me.get_jfloat(), w[k]); - const unsigned int crossSize = 10; - vpDisplay::displayCross(I, iP, crossSize, vpColor::cyan, 1); - } } ++k; // k contains good points and outliers (used for w[k]) } @@ -1045,36 +1003,15 @@ unsigned int vpMeEllipse::leastSquareRobust(const vpImage &I) ++numberOfGoodPoints; ++meList; ++angleList; - if (vpDEBUG_ENABLE(3)) { - vpImagePoint iP; - iP.set_ij(p_me.m_ifloat, p_me.m_jfloat); - const unsigned int crossSize = 10; - vpDisplay::displayCross(I, iP, crossSize, vpColor::red, 1); - printf("In LQR: angle : %lf, i = %.0lf, j = %.0lf\n", vpMath::deg(new_ang), iP.get_i(), iP.get_j()); - } } else { meList = m_meList.erase(meList); angleList = m_angleList.erase(angleList); - if (vpDEBUG_ENABLE(3)) { - vpImagePoint iP; - iP.set_ij(p_me.m_ifloat, p_me.m_jfloat); - const unsigned int crossSize = 10; - vpDisplay::displayCross(I, iP, crossSize, vpColor::orange, 1); - printf("too near : angle %lf, i %.0f , j : %0.f\n", vpMath::deg(new_ang), p_me.get_ifloat(), p_me.get_jfloat()); - } } } else { // point not in the interval [alpha1 ; alpha2] meList = m_meList.erase(meList); angleList = m_angleList.erase(angleList); - if (vpDEBUG_ENABLE(3)) { - vpImagePoint iP; - iP.set_ij(p_me.m_ifloat, p_me.m_jfloat); - const unsigned int crossSize = 10; - vpDisplay::displayCross(I, iP, crossSize, vpColor::green, 1); - printf("not in interval: angle : %lf, i %.0f , j : %0.f\n", vpMath::deg(new_ang), p_me.get_ifloat(), p_me.get_jfloat()); - } } } @@ -1088,11 +1025,6 @@ unsigned int vpMeEllipse::leastSquareRobust(const vpImage &I) m_alphamin = m_angleList.front(); m_alphamax = m_angleList.back(); - if (vpDEBUG_ENABLE(3)) { - printf("alphamin : %lf, alphamax : %lf\n", vpMath::deg(m_alphamin), vpMath::deg(m_alphamax)); - printf("Fin leastSquareRobust : nb pts ok = %d \n", numberOfGoodPoints); - } - return numberOfGoodPoints; } @@ -1240,16 +1172,22 @@ void vpMeEllipse::initTracking(const vpImage &I, const std::vecto void vpMeEllipse::initTracking(const vpImage &I, const vpColVector ¶m, vpImagePoint *pt1, const vpImagePoint *pt2, bool trackCircle) { + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + m_trackCircle = trackCircle; if ((pt1 != nullptr) && (pt2 != nullptr)) { m_trackArc = true; } // useful for sample(I) : uc, vc, a, b, e, Ki, alpha1, alpha2 - m_uc = param[0]; - m_vc = param[1]; - m_n20 = param[2]; - m_n11 = param[3]; - m_n02 = param[4]; + m_uc = param[index_0]; + m_vc = param[index_1]; + m_n20 = param[index_2]; + m_n11 = param[index_3]; + m_n02 = param[index_4]; computeAbeFromNij(); computeKiFromNij(); @@ -1300,37 +1238,16 @@ void vpMeEllipse::track(const vpImage &I) // Compute the ellipse parameters from the tracked points, manage the lists, // and update the expected density ( m_numberOfGoodPoints = leastSquareRobust(I); - if (vpDEBUG_ENABLE(3)) { - printf("1st step: nb of Good points %u, density %d, alphamin %lf, alphamax %lf\n", - m_numberOfGoodPoints, m_expectedDensity, - vpMath::deg(m_alphamin), vpMath::deg(m_alphamax)); - } if (plugHoles(I) > 0) { m_numberOfGoodPoints = leastSquareRobust(I); // if new points have been added, recompute the ellipse parameters and manage again the lists - if (vpDEBUG_ENABLE(3)) { - printf("2nd step: nb of Good points %u, density %d, alphamin %lf, alphamax %lf\n", m_numberOfGoodPoints, m_expectedDensity, - vpMath::deg(m_alphamin), vpMath::deg(m_alphamax)); - } } const unsigned int minNbGoodPoints = 5; if (m_numberOfGoodPoints <= minNbGoodPoints) { - if (vpDEBUG_ENABLE(3)) { - printf("Before RESAMPLE !!! nb points %d \n", m_numberOfGoodPoints); - printf("A click to continue \n"); - vpDisplay::flush(I); - vpDisplay::getClick(I); - vpDisplay::display(I); - } sample(I); vpMeTracker::track(I); leastSquareRobust(I); - if (vpDEBUG_ENABLE(3)) { - printf("nb of Good points %u, density %d %lf, alphamin %lf, alphamax\n", - m_numberOfGoodPoints, m_expectedDensity, - vpMath::deg(m_alphamin), vpMath::deg(m_alphamax)); - } // Stop in case of failure after resample if (m_numberOfGoodPoints <= minNbGoodPoints) { @@ -1338,10 +1255,6 @@ void vpMeEllipse::track(const vpImage &I) } } - if (vpDEBUG_ENABLE(3)) { - printParameters(); - } - // remet a jour l'angle delta pour chaque vpMeSite de la liste updateTheta(); // not in getParameters since computed only once for each image @@ -1350,12 +1263,6 @@ void vpMeEllipse::track(const vpImage &I) // Useful only for tracking an arc of ellipse, but done to give them a value computePointOnEllipse(m_alpha1, m_iP1); computePointOnEllipse(m_alpha2, m_iP2); - - if (vpDEBUG_ENABLE(3)) { - display(I, vpColor::red); - vpMeTracker::display(I); - vpDisplay::flush(I); - } } #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS diff --git a/modules/tracker/me/src/moving-edges/vpMeLine.cpp b/modules/tracker/me/src/moving-edges/vpMeLine.cpp index acc7c8226c..43aafd0b3d 100644 --- a/modules/tracker/me/src/moving-edges/vpMeLine.cpp +++ b/modules/tracker/me/src/moving-edges/vpMeLine.cpp @@ -40,6 +40,7 @@ #include // std::fabs #include // numeric_limits #include +#include #include #include #include diff --git a/modules/tracker/me/src/moving-edges/vpMeSite.cpp b/modules/tracker/me/src/moving-edges/vpMeSite.cpp index 3b5abdbb23..d7fe541c1e 100644 --- a/modules/tracker/me/src/moving-edges/vpMeSite.cpp +++ b/modules/tracker/me/src/moving-edges/vpMeSite.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -284,10 +284,11 @@ void vpMeSite::track(const vpImage &I, const vpMe *me, const bool // array in which likelihood ratios will be stored double *likelihood = new double[(2 * range) + 1]; + const unsigned int val_2 = 2; if (test_contrast) { double diff = 1e6; - for (unsigned int n = 0; n < ((2 * range) + 1); ++n) { + for (unsigned int n = 0; n < ((val_2 * range) + 1); ++n) { // convolution results double convolution_ = list_query_pixels[n].convolution(I, me); double threshold = list_query_pixels[n].getContrastThreshold(); @@ -317,7 +318,7 @@ void vpMeSite::track(const vpImage &I, const vpMe *me, const bool } } else { // test on contrast only - for (unsigned int n = 0; n < ((2 * range) + 1); ++n) { + for (unsigned int n = 0; n < ((val_2 * range) + 1); ++n) { double threshold = list_query_pixels[n].getContrastThreshold(); if (me->getLikelihoodThresholdType() == vpMe::NORMALIZED_THRESHOLD) { @@ -330,7 +331,7 @@ void vpMeSite::track(const vpImage &I, const vpMe *me, const bool // convolution results double convolution_ = list_query_pixels[n].convolution(I, me); - likelihood[n] = fabs(2 * convolution_); + likelihood[n] = fabs(val_2 * convolution_); if ((likelihood[n] > max) && (likelihood[n] > threshold)) { max_convolution = convolution_; max = likelihood[n]; @@ -416,29 +417,30 @@ void vpMeSite::display(const vpImage &I, const double &i, const d void vpMeSite::display(const vpImage &I, const double &i, const double &j, const vpMeSiteState &state) { + const unsigned int cross_size = 3; switch (state) { case NO_SUPPRESSION: - vpDisplay::displayCross(I, vpImagePoint(i, j), 3, vpColor::green, 1); + vpDisplay::displayCross(I, vpImagePoint(i, j), cross_size, vpColor::green, 1); break; case CONTRAST: - vpDisplay::displayCross(I, vpImagePoint(i, j), 3, vpColor::blue, 1); + vpDisplay::displayCross(I, vpImagePoint(i, j), cross_size, vpColor::blue, 1); break; case THRESHOLD: - vpDisplay::displayCross(I, vpImagePoint(i, j), 3, vpColor::purple, 1); + vpDisplay::displayCross(I, vpImagePoint(i, j), cross_size, vpColor::purple, 1); break; case M_ESTIMATOR: - vpDisplay::displayCross(I, vpImagePoint(i, j), 3, vpColor::red, 1); + vpDisplay::displayCross(I, vpImagePoint(i, j), cross_size, vpColor::red, 1); break; case OUTSIDE_ROI_MASK: - vpDisplay::displayCross(I, vpImagePoint(i, j), 3, vpColor::cyan, 1); + vpDisplay::displayCross(I, vpImagePoint(i, j), cross_size, vpColor::cyan, 1); break; default: - vpDisplay::displayCross(I, vpImagePoint(i, j), 3, vpColor::yellow, 1); + vpDisplay::displayCross(I, vpImagePoint(i, j), cross_size, vpColor::yellow, 1); } } diff --git a/modules/tracker/me/src/moving-edges/vpMeTracker.cpp b/modules/tracker/me/src/moving-edges/vpMeTracker.cpp index d6695b4e3c..9c71d04a9c 100644 --- a/modules/tracker/me/src/moving-edges/vpMeTracker.cpp +++ b/modules/tracker/me/src/moving-edges/vpMeTracker.cpp @@ -41,18 +41,15 @@ #include #include -#include #include BEGIN_VISP_NAMESPACE -#define DEBUG_LEVEL1 0 -#define DEBUG_LEVEL2 0 - void vpMeTracker::init() { vpTracker::init(); - p.resize(2); + const unsigned int val_2 = 2; + p.resize(val_2); m_selectDisplay = vpMeSite::NONE; } @@ -185,7 +182,6 @@ bool vpMeTracker::outOfImage(const vpImagePoint &iP, int border, int nrows, int void vpMeTracker::initTracking(const vpImage &I) { if (!m_me) { - vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized"); throw(vpTrackingException(vpTrackingException::initializationError, "Moving edges not initialized")); } @@ -219,12 +215,10 @@ void vpMeTracker::initTracking(const vpImage &I) void vpMeTracker::track(const vpImage &I) { if (!m_me) { - vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized"); throw(vpTrackingException(vpTrackingException::initializationError, "Moving edges not initialized")); } if (m_meList.empty()) { - vpDERROR_TRACE(2, "Tracking error: too few pixel to track"); throw(vpTrackingException(vpTrackingException::notEnoughPointError, "Too few pixel to track")); } @@ -291,7 +285,4 @@ void vpMeTracker::display(const vpImage &I, vpColVector &w, unsig display(I); } -#undef DEBUG_LEVEL1 -#undef DEBUG_LEVEL2 - END_VISP_NAMESPACE diff --git a/modules/tracker/tt/src/warp/vpTemplateTrackerWarp.cpp b/modules/tracker/tt/src/warp/vpTemplateTrackerWarp.cpp index adde6be834..41a8e93e19 100644 --- a/modules/tracker/tt/src/warp/vpTemplateTrackerWarp.cpp +++ b/modules/tracker/tt/src/warp/vpTemplateTrackerWarp.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,12 +29,9 @@ * * Description: * Template tracker. - * - * Authors: - * Amaury Dame - * Aurelien Yol - * -*****************************************************************************/ + */ + +#include #include BEGIN_VISP_NAMESPACE diff --git a/modules/vision/include/visp3/vision/vpBasicKeyPoint.h b/modules/vision/include/visp3/vision/vpBasicKeyPoint.h index 47c065d8b5..bead986e96 100644 --- a/modules/vision/include/visp3/vision/vpBasicKeyPoint.h +++ b/modules/vision/include/visp3/vision/vpBasicKeyPoint.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,14 +31,15 @@ * Key point used in matching algorithm. */ -#ifndef vpBasicKeyPoint_H -#define vpBasicKeyPoint_H +#ifndef VP_BASIC_KEYPOINT_H +#define VP_BASIC_KEYPOINT_H /*! * \file vpBasicKeyPoint.h * \brief Class that defines what is a keypoint. */ +#include #include #include #include diff --git a/modules/vision/include/visp3/vision/vpHomography.h b/modules/vision/include/visp3/vision/vpHomography.h index 7503232b6a..fef78d3132 100644 --- a/modules/vision/include/visp3/vision/vpHomography.h +++ b/modules/vision/include/visp3/vision/vpHomography.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -38,8 +38,8 @@ * some tools for homography computation. */ -#ifndef vpHomography_hh -#define vpHomography_hh +#ifndef VP_HOMOGRAPHY_H +#define VP_HOMOGRAPHY_H #include #include diff --git a/modules/vision/include/visp3/vision/vpPose.h b/modules/vision/include/visp3/vision/vpPose.h index faf3337222..d8b86e1c85 100644 --- a/modules/vision/include/visp3/vision/vpPose.h +++ b/modules/vision/include/visp3/vision/vpPose.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,18 +36,15 @@ * \brief Tools for pose computation (pose from point only). */ -#ifndef _vpPose_h_ -#define _vpPose_h_ +#ifndef VP_POSE_H +#define VP_POSE_H #include #include #include +#include #include #include -#include -#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS -#include -#endif #include #include @@ -119,6 +116,9 @@ class VISP_EXPORT vpPose double residual; //!< Residual in meter public: + // Typedef a function that checks if a pose is valid. + typedef bool (*funcCheckValidityPose)(const vpHomogeneousMatrix &); + /*! * Default constructor. */ @@ -183,7 +183,7 @@ class VISP_EXPORT vpPose * has the smallest residual. * - vpPose::RANSAC: Robust Ransac aproach (doesn't need an initialization) */ - bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneousMatrix &) = nullptr); + bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, funcCheckValidityPose func = nullptr); /*! * @brief Method that first computes the pose \b cMo using the linear approaches of Dementhon and Lagrange @@ -326,7 +326,7 @@ class VISP_EXPORT vpPose * The number of threads used can then be set with setNbParallelRansacThreads(). * Filter flag can be used with setRansacFilterFlag(). */ - bool poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneousMatrix &) = nullptr); + bool poseRansac(vpHomogeneousMatrix &cMo, funcCheckValidityPose func = nullptr); /*! * Compute the pose using virtual visual servoing approach and @@ -440,8 +440,7 @@ class VISP_EXPORT vpPose vpMatrix getCovarianceMatrix() const { if (!computeCovariance) { - vpTRACE("Warning : The covariance matrix has not been computed. See " - "setCovarianceComputation() to do it."); + std::cout << "Warning: The covariance matrix has not been computed. See setCovarianceComputation() to do it." << std::endl; } return covarianceMatrix; } @@ -647,8 +646,9 @@ class VISP_EXPORT vpPose const unsigned int &numberOfInlierToReachAConsensus, const double &threshold, unsigned int &ninliers, std::vector &listInliers, vpHomogeneousMatrix &cMo, const int &maxNbTrials = 10000, bool useParallelRansac = true, unsigned int nthreads = 0, - bool (*func)(const vpHomogeneousMatrix &) = nullptr); + funcCheckValidityPose func = nullptr); +#ifdef VISP_HAVE_HOMOGRAPHY /*! * Carries out the camera pose the image of a rectangle and * the intrinsic parameters, the length on x axis is known but the @@ -670,6 +670,7 @@ class VISP_EXPORT vpPose */ static double poseFromRectangle(vpPoint &p1, vpPoint &p2, vpPoint &p3, vpPoint &p4, double lx, vpCameraParameters &cam, vpHomogeneousMatrix &cMo); +#endif // Check if std:c++17 or higher. // Here we cannot use (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) in the declaration of the class @@ -805,7 +806,7 @@ class VISP_EXPORT vpPose double ransacThreshold; //! Minimal distance point to plane to consider if the point belongs or not //! to the plane - double distanceToPlaneForCoplanarityTest; + double distToPlaneForCoplanarityTest; //! RANSAC flag to remove or not degenerate points RANSAC_FILTER_FLAGS ransacFlag; //! List of points used for the RANSAC (std::vector is contiguous whereas @@ -830,7 +831,7 @@ class VISP_EXPORT vpPose */ vpRansacFunctor(const vpHomogeneousMatrix &cMo_, unsigned int ransacNbInlierConsensus_, const int ransacMaxTrials_, double ransacThreshold_, unsigned int initial_seed_, bool checkDegeneratePoints_, - const std::vector &listOfUniquePoints_, bool (*func_)(const vpHomogeneousMatrix &)) + const std::vector &listOfUniquePoints_, funcCheckValidityPose func_) : m_best_consensus(), m_checkDegeneratePoints(checkDegeneratePoints_), m_cMo(cMo_), m_foundSolution(false), m_func(func_), m_listOfUniquePoints(listOfUniquePoints_), m_nbInliers(0), m_ransacMaxTrials(ransacMaxTrials_), m_ransacNbInlierConsensus(ransacNbInlierConsensus_), m_ransacThreshold(ransacThreshold_), @@ -867,7 +868,7 @@ class VISP_EXPORT vpPose bool m_checkDegeneratePoints; //!< Flag to check for degenerate points vpHomogeneousMatrix m_cMo; //!< Estimated pose bool m_foundSolution; //!< Solution found - bool (*m_func)(const vpHomogeneousMatrix &); //!< Pointer to ransac function + funcCheckValidityPose m_func; //!< Pointer to ransac function std::vector m_listOfUniquePoints; //!< List of unique points unsigned int m_nbInliers; //!< Number of inliers int m_ransacMaxTrials; //!< Ransac max trial number diff --git a/modules/vision/include/visp3/vision/vpPoseException.h b/modules/vision/include/visp3/vision/vpPoseException.h index 00172ef0fd..a3df88da52 100644 --- a/modules/vision/include/visp3/vision/vpPoseException.h +++ b/modules/vision/include/visp3/vision/vpPoseException.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,8 +31,8 @@ * Error that can be emitted by the vpPose class and its derivatives */ -#ifndef _vpPoseException_h_ -#define _vpPoseException_h_ +#ifndef VP_POSE_EXCEPTION_H +#define VP_POSE_EXCEPTION_H #include diff --git a/modules/vision/src/calibration/vpCalibrationTools.cpp b/modules/vision/src/calibration/vpCalibrationTools.cpp index 35ce75a0e5..5d3e908265 100644 --- a/modules/vision/src/calibration/vpCalibrationTools.cpp +++ b/modules/vision/src/calibration/vpCalibrationTools.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,6 +31,7 @@ * Camera calibration. */ +#include #include #include #include @@ -38,12 +39,6 @@ #include // std::fabs -#define DEBUG_LEVEL1 0 -#define DEBUG_LEVEL2 0 - -#undef MAX /* FC unused anywhere */ -#undef MIN /* FC unused anywhere */ - BEGIN_VISP_NAMESPACE void vpCalibration::calibLagrange(vpCameraParameters &cam_est, vpHomogeneousMatrix &cMo_est) { @@ -1223,5 +1218,3 @@ void vpCalibration::calibVVSWithDistortionMulti(unsigned int nbPose, vpCalibrati } } END_VISP_NAMESPACE -#undef DEBUG_LEVEL1 -#undef DEBUG_LEVEL2 diff --git a/modules/vision/src/homography-estimation/vpHomography.cpp b/modules/vision/src/homography-estimation/vpHomography.cpp index ccf7b944fc..2e9de1c506 100644 --- a/modules/vision/src/homography-estimation/vpHomography.cpp +++ b/modules/vision/src/homography-estimation/vpHomography.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/vision/src/homography-estimation/vpHomographyDLT.cpp b/modules/vision/src/homography-estimation/vpHomographyDLT.cpp index 46aea9a2f5..1b73058821 100644 --- a/modules/vision/src/homography-estimation/vpHomographyDLT.cpp +++ b/modules/vision/src/homography-estimation/vpHomographyDLT.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -174,16 +174,17 @@ void vpHomography::hartleyDenormalization(vpHomography &aHbn, vpHomography &aHb, T2T = T2.pseudoInverse(1e-16); vpMatrix aHbn_(3, 3); - for (unsigned int i = 0; i < 3; ++i) { - for (unsigned int j = 0; j < 3; ++j) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { + for (unsigned int j = 0; j < val_3; ++j) { aHbn_[i][j] = aHbn[i][j]; } } vpMatrix maHb = T2T * aHbn_ * T1; - for (unsigned int i = 0; i < 3; ++i) { - for (unsigned int j = 0; j < 3; ++j) { + for (unsigned int i = 0; i < val_3; ++i) { + for (unsigned int j = 0; j < val_3; ++j) { aHb[i][j] = maHb[i][j]; } } @@ -296,8 +297,9 @@ void vpHomography::DLT(const std::vector &xb, const std::vector h = V.getCol(indexSmallestSv); // build the homography - for (unsigned int i = 0; i < 3; ++i) { - for (unsigned int j = 0; j < 3; ++j) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { + for (unsigned int j = 0; j < val_3; ++j) { aHbn[i][j] = h[(3 * i) + j]; } } diff --git a/modules/vision/src/homography-estimation/vpHomographyExtract.cpp b/modules/vision/src/homography-estimation/vpHomographyExtract.cpp index 2ebec6a37c..962f320918 100644 --- a/modules/vision/src/homography-estimation/vpHomographyExtract.cpp +++ b/modules/vision/src/homography-estimation/vpHomographyExtract.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -114,8 +114,9 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, const vpColVecto vpMatrix aRbp(3, 3); vpMatrix mH(3, 3); - for (unsigned int i = 0; i < 3; ++i) { - for (unsigned int j = 0; j < 3; ++j) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { + for (unsigned int j = 0; j < val_3; ++j) { mH[i][j] = aHb[i][j]; } } @@ -177,9 +178,9 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, const vpColVecto hypothese : sv[0]>=sv[1]>=sv[2]>=0 *****/ - for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int i = 0; i < val_3; ++i) { sv[i] = svTemp[vOrdre[i]]; - for (unsigned int j = 0; j < 3; ++j) { + for (unsigned int j = 0; j < val_3; ++j) { mU[i][j] = mTempU[i][vOrdre[j]]; mV[i][j] = mTempV[i][vOrdre[j]]; } @@ -237,7 +238,7 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, const vpColVecto /***** test pour determiner quelle est la bonne solution on teste d'abord quelle solution est plus proche de la perpendiculaire - au plan de la cible constuction de la normale n + au plan de la cible construction de la normale n *****/ /***** @@ -258,16 +259,16 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, const vpColVecto for (unsigned int k = 0; k < 2; ++k) { /* Pour le signe */ /* Calcul de la normale estimee : n = V.n' */ - for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int i = 0; i < val_3; ++i) { normaleEstimee[i] = 0.0; - for (unsigned int j = 0; j < 3; ++j) { + for (unsigned int j = 0; j < val_3; ++j) { normaleEstimee[i] += ((2.0 * k) - 1.0) * mV[i][j] * mX[j][w]; } } /* Calcul du cosinus de l'angle entre la normale reelle et desire */ double cosinusDesireeEstimee = 0.0; - for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int i = 0; i < val_3; ++i) { cosinusDesireeEstimee += normaleEstimee[i] * normaleDesiree[i]; } @@ -279,7 +280,7 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, const vpColVecto cosinusAncien = cosinusDesireeEstimee; /* Affectation de la normale qui est retourner */ - for (unsigned int j = 0; j < 3; ++j) { + for (unsigned int j = 0; j < val_3; ++j) { n[j] = normaleEstimee[j]; } @@ -301,9 +302,9 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, const vpColVecto } /* fin else */ /* Calcul du vecteur de translation qui est retourner : t = (U * t') / d */ - for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int i = 0; i < val_3; ++i) { atb[i] = 0.0; - for (unsigned int j = 0; j < 3; ++j) { + for (unsigned int j = 0; j < val_3; ++j) { atb[i] += mU[i][j] * aTbp[j]; } atb[i] /= distanceFictive; @@ -342,20 +343,20 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, const vpColVecto aRbp[2][2] = cosinusTheta; /* multiplication Rint = U R' */ - for (unsigned int i = 0; i < 3; ++i) { - for (unsigned int j = 0; j < 3; ++j) { + for (unsigned int i = 0; i < val_3; ++i) { + for (unsigned int j = 0; j < val_3; ++j) { aRbint[i][j] = 0.0; - for (unsigned int k = 0; k < 3; ++k) { + for (unsigned int k = 0; k < val_3; ++k) { aRbint[i][j] += mU[i][k] * aRbp[k][j]; } } } /* multiplication R = Rint . V^T */ - for (unsigned int i = 0; i < 3; ++i) { - for (unsigned int j = 0; j < 3; ++j) { + for (unsigned int i = 0; i < val_3; ++i) { + for (unsigned int j = 0; j < val_3; ++j) { aRb[i][j] = 0.0; - for (unsigned int k = 0; k < 3; ++k) { + for (unsigned int k = 0; k < val_3; ++k) { aRb[i][j] += aRbint[i][k] * mV[j][k]; } } @@ -417,8 +418,9 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, vpRotationMatrix vpMatrix aRbp(3, 3); vpMatrix mH(3, 3); - for (unsigned int i = 0; i < 3; ++i) { - for (unsigned int j = 0; j < 3; ++j) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { + for (unsigned int j = 0; j < val_3; ++j) { mH[i][j] = aHb[i][j]; } } @@ -480,9 +482,9 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, vpRotationMatrix hypothese : sv[0]>=sv[1]>=sv[2]>=0 *****/ - for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int i = 0; i < val_3; ++i) { sv[i] = svTemp[vOrdre[i]]; - for (unsigned int j = 0; j < 3; ++j) { + for (unsigned int j = 0; j < val_3; ++j) { mU[i][j] = mTempU[i][vOrdre[j]]; mV[i][j] = mTempV[i][vOrdre[j]]; } @@ -561,16 +563,16 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, vpRotationMatrix for (unsigned int k = 0; k < 2; ++k) { /* Pour le signe */ /* Calcul de la normale estimee : n = V.n' */ - for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int i = 0; i < val_3; ++i) { normaleEstimee[i] = 0.0; - for (unsigned int j = 0; j < 3; ++j) { + for (unsigned int j = 0; j < val_3; ++j) { normaleEstimee[i] += ((2.0 * k)- 1.0) * mV[i][j] * mX[j][w]; } } /* Calcul du cosinus de l'angle entre la normale reelle et desire */ double cosinusDesireeEstimee = 0.0; - for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int i = 0; i < val_3; ++i) { cosinusDesireeEstimee += normaleEstimee[i] * normaleDesiree[i]; } @@ -582,7 +584,7 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, vpRotationMatrix cosinusAncien = cosinusDesireeEstimee; /* Affectation de la normale qui est retourner */ - for (unsigned int j = 0; j < 3; ++j) { + for (unsigned int j = 0; j < val_3; ++j) { n[j] = normaleEstimee[j]; } @@ -604,9 +606,9 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, vpRotationMatrix } /* fin else */ /* Calcul du vecteur de translation qui est retourner : t = (U * t') / d */ - for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int i = 0; i < val_3; ++i) { atb[i] = 0.0; - for (unsigned int j = 0; j < 3; ++j) { + for (unsigned int j = 0; j < val_3; ++j) { atb[i] += mU[i][j] * aTbp[j]; } atb[i] /= distanceFictive; @@ -645,20 +647,20 @@ void vpHomography::computeDisplacement(const vpHomography &aHb, vpRotationMatrix aRbp[2][2] = cosinusTheta; /* multiplication Rint = U R' */ - for (unsigned int i = 0; i < 3; ++i) { - for (unsigned int j = 0; j < 3; ++j) { + for (unsigned int i = 0; i < val_3; ++i) { + for (unsigned int j = 0; j < val_3; ++j) { aRbint[i][j] = 0.0; - for (unsigned int k = 0; k < 3; ++k) { + for (unsigned int k = 0; k < val_3; ++k) { aRbint[i][j] += mU[i][k] * aRbp[k][j]; } } } /* multiplication R = Rint . V^T */ - for (unsigned int i = 0; i < 3; ++i) { - for (unsigned int j = 0; j < 3; ++j) { + for (unsigned int i = 0; i < val_3; ++i) { + for (unsigned int j = 0; j < val_3; ++j) { aRb[i][j] = 0.0; - for (unsigned int k = 0; k < 3; ++k) { + for (unsigned int k = 0; k < val_3; ++k) { aRb[i][j] += aRbint[i][k] * mV[j][k]; } } @@ -688,6 +690,7 @@ void vpHomography::computeDisplacement(const vpHomography &H, double x, double y bool norm1ok = false, norm2ok = false, norm3ok = false, norm4ok = false; double tmp, determinantU, determinantV, s, distanceFictive; + const unsigned int val_3 = 3; vpMatrix mTempU(3, 3), mTempV(3, 3), U(3, 3), V(3, 3); vpRotationMatrix Rprim1, R1; @@ -764,9 +767,9 @@ void vpHomography::computeDisplacement(const vpHomography &H, double x, double y en fonction des valeurs singulieres car hypothese : sv[0]>=sv[1]>=sv[2]>=0 *****/ - for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int i = 0; i < val_3; ++i) { sv[i] = svTemp[vOrdre[i]]; - for (unsigned int j = 0; j < 3; ++j) { + for (unsigned int j = 0; j < val_3; ++j) { U[i][j] = mTempU[i][vOrdre[j]]; V[i][j] = mTempV[i][vOrdre[j]]; } diff --git a/modules/vision/src/homography-estimation/vpHomographyMalis.cpp b/modules/vision/src/homography-estimation/vpHomographyMalis.cpp index 863f0c0cb0..d1b5a088b4 100644 --- a/modules/vision/src/homography-estimation/vpHomographyMalis.cpp +++ b/modules/vision/src/homography-estimation/vpHomographyMalis.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -98,8 +98,9 @@ void changeFrame(unsigned int *pts_ref, unsigned int nb_pts, vpMatrix &pd, vpMat vpMatrix Md(3, 3); vpMatrix Mp(3, 3); - for (unsigned int i = 0; i < 3; ++i) { - for (unsigned int j = 0; j < 3; ++j) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { + for (unsigned int j = 0; j < val_3; ++j) { M[j][i] = p[pts_ref[i]][j]; Md[j][i] = pd[pts_ref[i]][j]; } @@ -113,15 +114,15 @@ void changeFrame(unsigned int *pts_ref, unsigned int nb_pts, vpMatrix &pd, vpMat double lamb_des[3]; double lamb_cour[3]; - for (unsigned int i = 0; i < 3; ++i) { - for (unsigned int j = 0; j < 3; ++j) { + for (unsigned int i = 0; i < val_3; ++i) { + for (unsigned int j = 0; j < val_3; ++j) { lamb_cour[i] = Mp[i][j] * p[pts_ref[3]][j]; lamb_des[i] = Mdp[i][j] * pd[pts_ref[3]][j]; } } - for (unsigned int i = 0; i < 3; ++i) { - for (unsigned int j = 0; j < 3; ++j) { + for (unsigned int i = 0; i < val_3; ++i) { + for (unsigned int j = 0; j < val_3; ++j) { M[i][j] = M[i][j] * lamb_cour[j]; Md[i][j] = Md[i][j] * lamb_des[j]; } @@ -136,10 +137,10 @@ void changeFrame(unsigned int *pts_ref, unsigned int nb_pts, vpMatrix &pd, vpMat unsigned int cont_pts = 0; for (unsigned int k = 0; k < nb_pts; ++k) { if ((pts_ref[0] != k) && (pts_ref[1] != k) && (pts_ref[2] != k)) { - for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int i = 0; i < val_3; ++i) { pn[cont_pts][i] = 0.0; pnd[cont_pts][i] = 0.0; - for (unsigned int j = 0; j < 3; ++j) { + for (unsigned int j = 0; j < val_3; ++j) { pn[cont_pts][i] = pn[cont_pts][i] + (Mp[i][j] * p[k][j]); pnd[cont_pts][i] = pnd[cont_pts][i] + (Mdp[i][j] * pd[k][j]); } @@ -254,8 +255,9 @@ void hlm2D(unsigned int nb_pts, vpMatrix &points_des, vpMatrix &points_cour, vpM H.resize(3, 3); /** construction de la matrice H **/ - for (unsigned int i = 0; i < 3; ++i) { - for (unsigned int j = 0; j < 3; ++j) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { + for (unsigned int j = 0; j < val_3; ++j) { H[i][j] = V[(3 * i) + j][vect]; } } @@ -544,7 +546,8 @@ void hlm3D(unsigned int nb_pts, vpMatrix &pd, vpMatrix &p, vpMatrix &H) T[8][2] = V[2][vect]; vpMatrix Hd(3, 3); // diag(gu,gv,gw) - for (unsigned int i = 0; i < 3; ++i) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { Hd[i][i] = H_nr[i]; } diff --git a/modules/vision/src/homography-estimation/vpHomographyRansac.cpp b/modules/vision/src/homography-estimation/vpHomographyRansac.cpp index 52c09b479b..bfa4dab488 100644 --- a/modules/vision/src/homography-estimation/vpHomographyRansac.cpp +++ b/modules/vision/src/homography-estimation/vpHomographyRansac.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,6 +31,7 @@ * Homography estimation. */ +#include #include #include #include @@ -39,14 +40,14 @@ #include #include -BEGIN_VISP_NAMESPACE - #define VPEPS 1e-6 - /*! - \file vpHomographyRansac.cpp - \brief function used to estimate an homography using the Ransac algorithm - */ +BEGIN_VISP_NAMESPACE + +/*! + \file vpHomographyRansac.cpp + \brief function used to estimate an homography using the Ransac algorithm +*/ #ifndef DOXYGEN_SHOULD_SKIP_THIS @@ -171,11 +172,12 @@ bool vpHomography::degenerateConfiguration(const vpColVector &x, unsigned int *i } } - unsigned int n = x.getRows() / 4; + const unsigned int val_4 = 4; + unsigned int n = x.getRows() / val_4; double pa[4][3]; double pb[4][3]; unsigned int n2 = 2 * n; - for (unsigned int i = 0; i < 4; ++i) { + for (unsigned int i = 0; i < val_4; ++i) { unsigned int ind2 = 2 * ind[i]; pb[i][0] = x[ind2]; pb[i][1] = x[ind2 + 1]; @@ -252,11 +254,12 @@ bool vpHomography::degenerateConfiguration(const std::vector &xb, const // Fit model to this random selection of data points. void vpHomography::computeTransformation(vpColVector &x, unsigned int *ind, vpColVector &M) { - unsigned int n = x.getRows() / 4; + const unsigned int val_4 = 4; + unsigned int n = x.getRows() / val_4; std::vector xa(4), xb(4); std::vector ya(4), yb(4); unsigned int n2 = n * 2; - for (unsigned int i = 0; i < 4; ++i) { + for (unsigned int i = 0; i < val_4; ++i) { unsigned int ind2 = 2 * ind[i]; xb[i] = x[ind2]; yb[i] = x[ind2 + 1]; diff --git a/modules/vision/src/homography-estimation/vpHomographyVVS.cpp b/modules/vision/src/homography-estimation/vpHomographyVVS.cpp index 996f48d7e8..7963b2024b 100644 --- a/modules/vision/src/homography-estimation/vpHomographyVVS.cpp +++ b/modules/vision/src/homography-estimation/vpHomographyVVS.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -51,12 +51,13 @@ const double vpHomography::m_threshold_displacement = 1e-18; static void updatePoseRotation(vpColVector &dx, vpHomogeneousMatrix &mati) { vpRotationMatrix rd; + const unsigned int val_3 = 3; double s = sqrt((dx[0] * dx[0]) + (dx[1] * dx[1]) + (dx[2] * dx[2])); if (s > 1.e-25) { double u[3]; - for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int i = 0; i < val_3; ++i) { u[i] = dx[i] / s; } double sinu = sin(s); @@ -73,8 +74,8 @@ static void updatePoseRotation(vpColVector &dx, vpHomogeneousMatrix &mati) rd[2][2] = cosi + (mcosi * u[2] * u[2]); } else { - for (unsigned int i = 0; i < 3; ++i) { - for (unsigned int j = 0; j < 3; ++j) { + for (unsigned int i = 0; i < val_3; ++i) { + for (unsigned int j = 0; j < val_3; ++j) { rd[i][j] = 0.0; } rd[i][i] = 1.0; @@ -106,6 +107,7 @@ double vpHomography::computeRotation(unsigned int nbpoint, vpPoint *c1P, vpPoint bool only_1 = false; bool only_2 = false; int iter = 0; + const unsigned int val_3 = 3; unsigned int n = 0; for (unsigned int i = 0; i < nbpoint; ++i) { @@ -135,8 +137,8 @@ double vpHomography::computeRotation(unsigned int nbpoint, vpPoint *c1P, vpPoint // compute current position // Change frame (current) - for (unsigned int i = 0; i < 3; ++i) { - for (unsigned int j = 0; j < 3; ++j) { + for (unsigned int i = 0; i < val_3; ++i) { + for (unsigned int j = 0; j < val_3; ++j) { c2Rc1[i][j] = c2Mc1[i][j]; } } @@ -250,7 +252,7 @@ double vpHomography::computeRotation(unsigned int nbpoint, vpPoint *c1P, vpPoint vpColVector c2rc1, v(6); c2rc1 = -2 * Lp * W * e; - for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int i = 0; i < val_3; ++i) { v[i + 3] = c2rc1[i]; } diff --git a/modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.cpp b/modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.cpp index 968e3cd60c..6345c445da 100644 --- a/modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.cpp +++ b/modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -39,8 +39,6 @@ #include #include "vpLevenbergMarquartd.h" -BEGIN_VISP_NAMESPACE - #define SIGN(x) ((x) < 0 ? -1 : 1) #define SWAP(a, b, c) \ { \ @@ -52,6 +50,8 @@ BEGIN_VISP_NAMESPACE #define TRUE 1 #define FALSE 0 +BEGIN_VISP_NAMESPACE + double enorm(const double *x, int n) { const double rdwarf = 3.834e-20; @@ -1205,7 +1205,7 @@ int lmder1(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, return 0; } +END_VISP_NAMESPACE + #undef TRUE #undef FALSE - -END_VISP_NAMESPACE diff --git a/modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.h b/modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.h index b0daf95bf1..b4a4008373 100644 --- a/modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.h +++ b/modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,8 +31,8 @@ * Levenberg Marquartd. */ -#ifndef vpLevenbergMarquartd_h -#define vpLevenbergMarquartd_h +#ifndef VP_LEVENBERG_MARQUARTD_H +#define VP_LEVENBERG_MARQUARTD_H #include #include @@ -100,35 +100,35 @@ BEGIN_VISP_NAMESPACE * carres du systeme a * x = b, d * x = 0. sdiag Vecteur de taille "n" * contenant les elements diagonaux de la matrice triangulaire superieure "s". */ -int VISP_EXPORT qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, double *x, double *sdiag, - double *wa); + int VISP_EXPORT qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, double *x, double *sdiag, + double *wa); -/* - * PROCEDURE : enorm - * - * ENTREE : - * - * x Vecteur de taille "n" - * n Taille du vecteur "x" - * - * DESCRIPTION : - * La procedure calcule la norme euclidienne d'un vecteur "x" de taille "n" - * La norme euclidienne est calculee par accumulation de la somme des carres - * dans les trois directions. Les sommes des carres pour les petits et grands - * elements sont mis a echelle afin d'eviter les overflows. Des underflows non - * destructifs sont autorisee. Les underflows et overflows sont evites dans le - * calcul des sommes des carres non encore mis a echelle par les elements - * intermediaires. La definition des elements petit, intermediaire et grand - * depend de deux constantes : rdwarf et rdiant. Les restrictions principales - * sur ces constantes sont rdwarf^2 n'est pas en underflow et rdgiant^2 n'est - * pas en overflow. Les constantes donnees ici conviennent pour la plupart des - * pc connus. - * - * RETOUR : - * En cas de succes, la valeur retournee est la norme euclidienne du vecteur - * Sinon, la valeur -1 est retournee et la variable globale "errno" est - * initialisee pour indiquee le type de l'erreur. - */ + /* + * PROCEDURE : enorm + * + * ENTREE : + * + * x Vecteur de taille "n" + * n Taille du vecteur "x" + * + * DESCRIPTION : + * La procedure calcule la norme euclidienne d'un vecteur "x" de taille "n" + * La norme euclidienne est calculee par accumulation de la somme des carres + * dans les trois directions. Les sommes des carres pour les petits et grands + * elements sont mis a echelle afin d'eviter les overflows. Des underflows non + * destructifs sont autorisee. Les underflows et overflows sont evites dans le + * calcul des sommes des carres non encore mis a echelle par les elements + * intermediaires. La definition des elements petit, intermediaire et grand + * depend de deux constantes : rdwarf et rdiant. Les restrictions principales + * sur ces constantes sont rdwarf^2 n'est pas en underflow et rdgiant^2 n'est + * pas en overflow. Les constantes donnees ici conviennent pour la plupart des + * pc connus. + * + * RETOUR : + * En cas de succes, la valeur retournee est la norme euclidienne du vecteur + * Sinon, la valeur -1 est retournee et la variable globale "errno" est + * initialisee pour indiquee le type de l'erreur. + */ double VISP_EXPORT enorm(const double *x, int n); /* PROCEDURE : lmpar diff --git a/modules/vision/src/pose-estimation/vpPose.cpp b/modules/vision/src/pose-estimation/vpPose.cpp index bab5cc65fa..8cb23dec79 100644 --- a/modules/vision/src/pose-estimation/vpPose.cpp +++ b/modules/vision/src/pose-estimation/vpPose.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -37,7 +37,6 @@ */ #include -#include #include #include #include @@ -45,28 +44,36 @@ #include #include #include +#ifdef VISP_POSE_USES_HOMOGRAPHY +#include +#endif #include // std::fabs #include // numeric_limits BEGIN_VISP_NAMESPACE -#define DEBUG_LEVEL1 0 +namespace +{ +const int def_vvsIterMax = 200; +const unsigned int def_ransacNbInlier = 4; +const int def_ransacMaxTrials = 1000; +} vpPose::vpPose() - : npt(0), listP(), residual(0), m_lambda(0.9), m_dementhonSvThresh(1e-6), vvsIterMax(200), c3d(), + : npt(0), listP(), residual(0), m_lambda(0.9), m_dementhonSvThresh(1e-6), vvsIterMax(def_vvsIterMax), c3d(), computeCovariance(false), covarianceMatrix(), - ransacNbInlierConsensus(4), ransacMaxTrials(1000), ransacInliers(), ransacInlierIndex(), ransacThreshold(0.0001), - distanceToPlaneForCoplanarityTest(0.001), ransacFlag(vpPose::NO_FILTER), listOfPoints(), useParallelRansac(false), + ransacNbInlierConsensus(def_ransacNbInlier), ransacMaxTrials(def_ransacMaxTrials), ransacInliers(), ransacInlierIndex(), ransacThreshold(0.0001), + distToPlaneForCoplanarityTest(0.001), ransacFlag(vpPose::NO_FILTER), listOfPoints(), useParallelRansac(false), nbParallelRansacThreads(0), // 0 means that we use C++11 (if available) to get the number of threads vvsEpsilon(1e-8) { } vpPose::vpPose(const std::vector &lP) : npt(static_cast(lP.size())), listP(lP.begin(), lP.end()), residual(0), m_lambda(0.9), - m_dementhonSvThresh(1e-6), vvsIterMax(200), - c3d(), computeCovariance(false), covarianceMatrix(), ransacNbInlierConsensus(4), ransacMaxTrials(1000), - ransacInliers(), ransacInlierIndex(), ransacThreshold(0.0001), distanceToPlaneForCoplanarityTest(0.001), + m_dementhonSvThresh(1e-6), vvsIterMax(def_vvsIterMax), + c3d(), computeCovariance(false), covarianceMatrix(), ransacNbInlierConsensus(def_ransacNbInlier), ransacMaxTrials(def_ransacMaxTrials), + ransacInliers(), ransacInlierIndex(), ransacThreshold(0.0001), distToPlaneForCoplanarityTest(0.001), ransacFlag(vpPose::NO_FILTER), listOfPoints(lP), useParallelRansac(false), nbParallelRansacThreads(0), // 0 means that we use C++11 (if available) to get the number of threads vvsEpsilon(1e-8) @@ -74,15 +81,7 @@ vpPose::vpPose(const std::vector &lP) vpPose::~vpPose() { -#if (DEBUG_LEVEL1) - std::cout << "begin vpPose::~vpPose() " << std::endl; -#endif - listP.clear(); - -#if (DEBUG_LEVEL1) - std::cout << "end vpPose::~vpPose() " << std::endl; -#endif } void vpPose::clearPoint() @@ -106,7 +105,7 @@ void vpPose::addPoints(const std::vector &lP) npt = static_cast(listP.size()); } -void vpPose::setDistToPlaneForCoplanTest(double d) { distanceToPlaneForCoplanarityTest = d; } +void vpPose::setDistToPlaneForCoplanTest(double d) { distToPlaneForCoplanarityTest = d; } void vpPose::setDementhonSvThreshold(const double &svThresh) { @@ -119,12 +118,13 @@ void vpPose::setDementhonSvThreshold(const double &svThresh) bool vpPose::coplanar(int &coplanar_plane_type, double *p_a, double *p_b, double *p_c, double *p_d) { coplanar_plane_type = 0; - if (npt < 2) { - vpERROR_TRACE("Not enough point (%d) to compute the pose ", npt); - throw(vpPoseException(vpPoseException::notEnoughPointError, "Not enough points ")); + const unsigned int nbMinPt = 2; + if (npt < nbMinPt) { + throw(vpPoseException(vpPoseException::notEnoughPointError, "Not enough point (%d) to compute the pose ", npt)); } - if (npt == 3) { + const unsigned int nbPtPlan = 3; + if (npt == nbPtPlan) { return true; } @@ -230,7 +230,8 @@ bool vpPose::coplanar(int &coplanar_plane_type, double *p_a, double *p_b, double } if (degenerate) { - coplanar_plane_type = 4; // points are collinear + const int typeCollinear = 4; + coplanar_plane_type = typeCollinear; // points are collinear return true; } @@ -241,15 +242,18 @@ bool vpPose::coplanar(int &coplanar_plane_type, double *p_a, double *p_b, double if ((std::fabs(b) <= std::numeric_limits::epsilon()) && (std::fabs(c) <= std::numeric_limits::epsilon())) { - coplanar_plane_type = 1; // ax=d + const int typeAxD = 1; + coplanar_plane_type = typeAxD; // ax=d } else if ((std::fabs(a) <= std::numeric_limits::epsilon()) && (std::fabs(c) <= std::numeric_limits::epsilon())) { - coplanar_plane_type = 2; // by=d + const int typeByD = 2; + coplanar_plane_type = typeByD; // by=d } else if ((std::fabs(a) <= std::numeric_limits::epsilon()) && (std::fabs(b) <= std::numeric_limits::epsilon())) { - coplanar_plane_type = 3; // cz=d + const int typeCzD = 3; + coplanar_plane_type = typeCzD; // cz=d } double D = sqrt(vpMath::sqr(a) + vpMath::sqr(b) + vpMath::sqr(c)); @@ -259,14 +263,13 @@ bool vpPose::coplanar(int &coplanar_plane_type, double *p_a, double *p_b, double P1 = *it; double dist = ((a * P1.get_oX()) + (b * P1.get_oY()) + (c * P1.get_oZ()) + d) / D; - if (fabs(dist) > distanceToPlaneForCoplanarityTest) { - vpDEBUG_TRACE(10, " points are not coplanar "); + if (fabs(dist) > distToPlaneForCoplanarityTest) { + // points are not coplanar return false; } } - vpDEBUG_TRACE(10, " points are coplanar "); - // vpTRACE(" points are coplanar ") ; + // points are coplanar // If the points are coplanar and the input/output parameters are different from nullptr, // getting the values of the plan coefficient and storing in the input/output parameters @@ -334,23 +337,24 @@ double vpPose::computeResidual(const vpHomogeneousMatrix &cMo, const vpCameraPar double squaredResidual = vpMath::sqr(u_moved - u_initial) + vpMath::sqr(v_moved - v_initial); residuals[i] = squaredResidual; - i++; + ++i; squared_error += squaredResidual; } return squared_error; } -bool vpPose::computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneousMatrix &)) +bool vpPose::computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, funcCheckValidityPose func) { - if (npt < 4) { - throw(vpPoseException(vpPoseException::notEnoughPointError, "Not enough point (%d) to compute the pose ", npt)); - } + const int minNbPtDementhon = 4; + const int minNbPtRansac = 4; + const int minNbPtLagrangePlan = 4; + const int minNbPtLagrangeNoPlan = 6; switch (method) { case DEMENTHON: case DEMENTHON_VIRTUAL_VS: case DEMENTHON_LOWE: { - if (npt < 4) { + if (npt < minNbPtDementhon) { throw(vpPoseException(vpPoseException::notEnoughPointError, "Dementhon method cannot be used in that case " "(at least 4 points are required)" @@ -378,12 +382,12 @@ bool vpPose::computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool bool plan = coplanar(coplanar_plane_type, &a, &b, &c, &d); if (plan == true) { - - if (coplanar_plane_type == 4) { + const int typeCollinear = 4; + if (coplanar_plane_type == typeCollinear) { throw(vpPoseException(vpPoseException::notEnoughPointError, "Lagrange method cannot be used in that case " "(points are collinear)")); } - if (npt < 4) { + if (npt < minNbPtLagrangePlan) { throw(vpPoseException(vpPoseException::notEnoughPointError, "Lagrange method cannot be used in that case " "(at least 4 points are required). " @@ -393,7 +397,7 @@ bool vpPose::computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool poseLagrangePlan(cMo, &plan, &a, &b, &c, &d); } else { - if (npt < 6) { + if (npt < minNbPtLagrangeNoPlan) { throw(vpPoseException(vpPoseException::notEnoughPointError, "Lagrange method cannot be used in that case " "(at least 6 points are required when 3D points are non coplanar). " @@ -405,7 +409,7 @@ bool vpPose::computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool break; } case RANSAC: { - if (npt < 4) { + if (npt < minNbPtRansac) { throw(vpPoseException(vpPoseException::notEnoughPointError, "Ransac method cannot be used in that case " "(at least 4 points are required). " @@ -574,10 +578,11 @@ void vpPose::displayModel(vpImage &I, vpCameraParameters &cam, vp vpPoint P; vpImagePoint ip; std::list::const_iterator listp_end = listP.end(); + const unsigned int sizeCross = 5; for (std::list::const_iterator it = listP.begin(); it != listp_end; ++it) { P = *it; vpMeterPixelConversion::convertPoint(cam, P.p[0], P.p[1], ip); - vpDisplay::displayCross(I, ip, 5, col); + vpDisplay::displayCross(I, ip, sizeCross, col); } } @@ -586,46 +591,49 @@ void vpPose::displayModel(vpImage &I, vpCameraParameters &cam, vpColor c vpPoint P; vpImagePoint ip; std::list::const_iterator listp_end = listP.end(); + const unsigned int sizeCross = 5; + for (std::list::const_iterator it = listP.begin(); it != listp_end; ++it) { P = *it; vpMeterPixelConversion::convertPoint(cam, P.p[0], P.p[1], ip); - vpDisplay::displayCross(I, ip, 5, col); + vpDisplay::displayCross(I, ip, sizeCross, col); } } +#ifdef VISP_HAVE_HOMOGRAPHY double vpPose::poseFromRectangle(vpPoint &p1, vpPoint &p2, vpPoint &p3, vpPoint &p4, double lx, vpCameraParameters &cam, - vpHomogeneousMatrix &cMo) + vpHomogeneousMatrix &cMo) { - + const unsigned int id0 = 0, id1 = 1, id2 = 2, id3 = 3; std::vector rectx(4); std::vector recty(4); - rectx[0] = 0; - recty[0] = 0; - rectx[1] = 1; - recty[1] = 0; - rectx[2] = 1; - recty[2] = 1; - rectx[3] = 0; - recty[3] = 1; + rectx[id0] = 0; + recty[id0] = 0; + rectx[id1] = 1; + recty[id1] = 0; + rectx[id2] = 1; + recty[id2] = 1; + rectx[id3] = 0; + recty[id3] = 1; std::vector irectx(4); std::vector irecty(4); - irectx[0] = (p1.get_x()); - irecty[0] = (p1.get_y()); - irectx[1] = (p2.get_x()); - irecty[1] = (p2.get_y()); - irectx[2] = (p3.get_x()); - irecty[2] = (p3.get_y()); - irectx[3] = (p4.get_x()); - irecty[3] = (p4.get_y()); + irectx[id0] = (p1.get_x()); + irecty[id0] = (p1.get_y()); + irectx[id1] = (p2.get_x()); + irecty[id1] = (p2.get_y()); + irectx[id2] = (p3.get_x()); + irecty[id2] = (p3.get_y()); + irectx[id3] = (p4.get_x()); + irecty[id3] = (p4.get_y()); // calcul de l'homographie vpMatrix H(3, 3); vpHomography hom; - // --comment: vpHomography HartleyDLT of rectx recty irectx irecty hom vpHomography::HLM(rectx, recty, irectx, irecty, 1, hom); - for (unsigned int i = 0; i < 3; ++i) { - for (unsigned int j = 0; j < 3; ++j) { + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { + for (unsigned int j = 0; j < val_3; ++j) { H[i][j] = hom[i][j]; } } @@ -665,5 +673,6 @@ double vpPose::poseFromRectangle(vpPoint &p1, vpPoint &p2, vpPoint &p3, vpPoint P.computePose(vpPose::DEMENTHON_LOWE, cMo); return lx / s; } +#endif END_VISP_NAMESPACE diff --git a/modules/vision/src/pose-estimation/vpPoseDementhon.cpp b/modules/vision/src/pose-estimation/vpPoseDementhon.cpp index d05074fb5c..41349a9b98 100644 --- a/modules/vision/src/pose-estimation/vpPoseDementhon.cpp +++ b/modules/vision/src/pose-estimation/vpPoseDementhon.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -34,38 +34,29 @@ #include #include -BEGIN_VISP_NAMESPACE - -#define DEBUG_LEVEL1 0 -#define DEBUG_LEVEL2 0 -#define DEBUG_LEVEL3 0 - #define SEUIL_RESIDUAL 0.0001 /* avant 0.01 dans while du calculArbreDementhon */ #define EPS_DEM 0.001 - #define ITER_MAX 30 /* max number of iterations for Demonthon's loop */ +BEGIN_VISP_NAMESPACE + static void calculSolutionDementhon(vpColVector &I4, vpColVector &J4, vpHomogeneousMatrix &cMo) { - -#if (DEBUG_LEVEL1) - std::cout << "begin (Dementhon.cc)CalculSolutionDementhon() " << std::endl; -#endif - // norm of the 3 first components of I4 and J4 - double normI3 = sqrt((I4[0] * I4[0]) + (I4[1] * I4[1]) + (I4[2] * I4[2])); - double normJ3 = sqrt((J4[0] * J4[0]) + (J4[1] * J4[1]) + (J4[2] * J4[2])); + const int id0 = 0, id1 = 1, id2 = 2; + double normI3 = sqrt((I4[id0] * I4[id0]) + (I4[id1] * I4[id1]) + (I4[id2] * I4[id2])); + double normJ3 = sqrt((J4[id0] * J4[id0]) + (J4[id1] * J4[id1]) + (J4[id2] * J4[id2])); if ((normI3 < 1e-10) || (normJ3 < 1e-10)) { - // vpERROR_TRACE(" normI+normJ = 0, division par zero " ) ; throw(vpException(vpException::divideByZeroError, "Division by zero in Dementhon pose computation: normI or normJ = 0")); } double Z0 = 2.0 / (normI3 + normJ3); - vpColVector I3(3), J3(3), K3(3); - for (unsigned int i = 0; i < 3; ++i) { + const unsigned int sizeVectors = 3; + vpColVector I3(sizeVectors), J3(sizeVectors), K3(sizeVectors); + for (unsigned int i = 0; i < sizeVectors; ++i) { I3[i] = I4[i] / normI3; J3[i] = J4[i] / normJ3; } @@ -76,42 +67,41 @@ static void calculSolutionDementhon(vpColVector &I4, vpColVector &J4, vpHomogene J3 = vpColVector::cross(K3, I3); // calcul de la matrice de passage - cMo[0][0] = I3[0]; - cMo[0][1] = I3[1]; - cMo[0][2] = I3[2]; - cMo[0][3] = I4[3] * Z0; - - cMo[1][0] = J3[0]; - cMo[1][1] = J3[1]; - cMo[1][2] = J3[2]; - cMo[1][3] = J4[3] * Z0; - - cMo[2][0] = K3[0]; - cMo[2][1] = K3[1]; - cMo[2][2] = K3[2]; - cMo[2][3] = Z0; - -#if (DEBUG_LEVEL1) - std::cout << "end (Dementhon.cc)CalculSolutionDementhon() " << std::endl; -#endif + const unsigned int idX = 0, idY = 1, idZ = 2, idTranslation = 3; + cMo[idX][idX] = I3[idX]; + cMo[idX][idY] = I3[idY]; + cMo[idX][idZ] = I3[idZ]; + cMo[idX][idTranslation] = I4[idTranslation] * Z0; + + cMo[idY][idX] = J3[idX]; + cMo[idY][idY] = J3[idY]; + cMo[idY][idZ] = J3[idZ]; + cMo[idY][idTranslation] = J4[idTranslation] * Z0; + + cMo[idZ][idX] = K3[idX]; + cMo[idZ][idY] = K3[idY]; + cMo[idZ][idZ] = K3[idZ]; + cMo[idZ][idTranslation] = Z0; } void vpPose::poseDementhonNonPlan(vpHomogeneousMatrix &cMo) { vpPoint P; - double cdg[3]; + const unsigned int idX = 0, idY = 1, idZ = 2, size = 3; + double cdg[size]; /* compute the cog of the 3D points */ - cdg[0] = 0.0; - cdg[1] = 0.0; - cdg[2] = 0.0; + cdg[idX] = 0.0; + cdg[idY] = 0.0; + cdg[idZ] = 0.0; std::list::const_iterator listp_end = listP.end(); for (std::list::const_iterator it = listP.begin(); it != listp_end; ++it) { P = (*it); - cdg[0] += P.get_oX(); - cdg[1] += P.get_oY(); - cdg[2] += P.get_oZ(); + cdg[idX] += P.get_oX(); + cdg[idY] += P.get_oY(); + cdg[idZ] += P.get_oZ(); } - for (unsigned int i = 0; i < 3; ++i) { + + for (unsigned int i = 0; i < size; ++i) { cdg[i] /= npt; } // --comment: print cdg cdg of 0 cdg of 1 cdg of 2 @@ -121,29 +111,24 @@ void vpPose::poseDementhonNonPlan(vpHomogeneousMatrix &cMo) std::list::const_iterator listp_end_s = listP.end(); for (std::list::const_iterator it = listP.begin(); it != listp_end_s; ++it) { P = (*it); - P.set_oX(P.get_oX() - cdg[0]); - P.set_oY(P.get_oY() - cdg[1]); - 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); } - vpMatrix A(npt, 4), Ap; + const unsigned int idHomogeneousCoord = 3; + const unsigned int homogeneousCoordSize = 4; + vpMatrix A(npt, homogeneousCoordSize), Ap; for (unsigned int i = 0; i < npt; ++i) { - A[i][0] = c3d[i].get_oX(); - A[i][1] = c3d[i].get_oY(); - A[i][2] = c3d[i].get_oZ(); - A[i][3] = 1.0; + A[i][idX] = c3d[i].get_oX(); + A[i][idY] = c3d[i].get_oY(); + A[i][idZ] = c3d[i].get_oZ(); + A[i][idHomogeneousCoord] = 1.0; } Ap = A.pseudoInverse(m_dementhonSvThresh); -#if (DEBUG_LEVEL2) - { - std::cout << "A" << std::endl << A << std::endl; - std::cout << "A^+" << std::endl << Ap << std::endl; - } -#endif - vpColVector xprim(npt); vpColVector yprim(npt); @@ -151,17 +136,18 @@ void vpPose::poseDementhonNonPlan(vpHomogeneousMatrix &cMo) xprim[i] = c3d[i].get_x(); yprim[i] = c3d[i].get_y(); } - vpColVector I4(4), J4(4); + vpColVector I4(homogeneousCoordSize), J4(homogeneousCoordSize); I4 = Ap * xprim; J4 = Ap * yprim; calculSolutionDementhon(I4, J4, cMo); + const unsigned int idTranslation = 3; int erreur = 0; for (unsigned int i = 0; i < npt; ++i) { double z; - z = (cMo[2][0] * c3d[i].get_oX()) + (cMo[2][1] * c3d[i].get_oY()) + (cMo[2][2] * c3d[i].get_oZ()) + cMo[2][3]; + z = (cMo[idZ][idX] * c3d[i].get_oX()) + (cMo[idZ][idY] * c3d[i].get_oY()) + (cMo[idZ][idZ] * c3d[i].get_oZ()) + cMo[idZ][idTranslation]; if (z <= 0.0) { erreur = -1; } @@ -184,7 +170,7 @@ void vpPose::poseDementhonNonPlan(vpHomogeneousMatrix &cMo) for (unsigned int i = 0; i < npt; ++i) { double eps = - ((cMo[2][0] * c3d[i].get_oX()) + (cMo[2][1] * c3d[i].get_oY()) + (cMo[2][2] * c3d[i].get_oZ())) / cMo[2][3]; + ((cMo[idZ][idX] * c3d[i].get_oX()) + (cMo[idZ][idY] * c3d[i].get_oY()) + (cMo[idZ][idZ] * c3d[i].get_oZ())) / cMo[idZ][idTranslation]; xprim[i] = (1.0 + eps) * c3d[i].get_x(); yprim[i] = (1.0 + eps) * c3d[i].get_y(); @@ -196,7 +182,7 @@ void vpPose::poseDementhonNonPlan(vpHomogeneousMatrix &cMo) res = sqrt(computeResidualDementhon(cMo) / npt); for (unsigned int i = 0; i < npt; ++i) { double z; - z = (cMo[2][0] * c3d[i].get_oX()) + (cMo[2][1] * c3d[i].get_oY()) + (cMo[2][2] * c3d[i].get_oZ()) + cMo[2][3]; + z = (cMo[idZ][idX] * c3d[i].get_oX()) + (cMo[idZ][idY] * c3d[i].get_oY()) + (cMo[idZ][idZ] * c3d[i].get_oZ()) + cMo[idZ][idTranslation]; if (z <= 0.0) { erreur = -1; } @@ -204,28 +190,17 @@ void vpPose::poseDementhonNonPlan(vpHomogeneousMatrix &cMo) if (erreur == -1) { cMo = cMo_old; res = res_old; // to leave the while loop -#if (DEBUG_LEVEL3) - std::cout << "Pb z < 0 with cMo in Dementhon's loop" << std::endl; -#endif } -#if (DEBUG_LEVEL3) - vpThetaUVector erc; - cMo.extract(erc); - std::cout << "it = " << cpt << " residu = " << res << " Theta U rotation: " << vpMath::deg(erc[0]) << " " - << vpMath::deg(erc[1]) << " " << vpMath::deg(erc[2]) << std::endl; -#endif + if (res > res_old) { -#if (DEBUG_LEVEL3) - std::cout << "Divergence : res = " << res << " res_old = " << res_old << std::endl; -#endif cMo = cMo_old; } ++cpt; } // go back to the initial frame - cMo[0][3] -= ((cdg[0] * cMo[0][0]) + (cdg[1] * cMo[0][1]) + (cdg[2] * cMo[0][2])); - cMo[1][3] -= ((cdg[0] * cMo[1][0]) + (cdg[1] * cMo[1][1]) + (cdg[2] * cMo[1][2])); - cMo[2][3] -= ((cdg[0] * cMo[2][0]) + (cdg[1] * cMo[2][1]) + (cdg[2] * cMo[2][2])); + 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])); } static void calculRTheta(double s, double c, double &r, double &theta) @@ -238,10 +213,10 @@ static void calculRTheta(double s, double c, double &r, double &theta) if (fabs(c) > fabs(s)) { r = fabs(c); if (c >= 0.0) { - theta = M_PI / 2; + theta = M_PI / 2.; } else { - theta = -M_PI / 2; + theta = -M_PI / 2.; } } else { @@ -259,8 +234,9 @@ static void calculRTheta(double s, double c, double &r, double &theta) static void calculTwoSolutionsDementhonPlan(vpColVector &I04, vpColVector &J04, vpColVector &U, vpHomogeneousMatrix &cMo1, vpHomogeneousMatrix &cMo2) { - vpColVector I0(3), J0(3); - for (unsigned int i = 0; i < 3; ++i) { + const unsigned int size = 3; + vpColVector I0(size), J0(size); + for (unsigned int i = 0; i < size; ++i) { I0[i] = I04[i]; J0[i] = J04[i]; } @@ -273,44 +249,29 @@ static void calculTwoSolutionsDementhonPlan(vpColVector &I04, vpColVector &J04, double si = sin(theta); // calcul de la premiere solution - vpColVector I(4), J(4); + const unsigned int sizeHomogeneous = 4; + vpColVector I(sizeHomogeneous), J(sizeHomogeneous); I = I04 + (U * r * co); J = J04 + (U * r * si); -#if (DEBUG_LEVEL2) - { - std::cout << "I0 " << I04.t() << std::endl; - std::cout << "J0 " << J04.t() << std::endl; - std::cout << "I1 " << I.t() << std::endl; - std::cout << "J1 " << J.t() << std::endl; - } -#endif calculSolutionDementhon(I, J, cMo1); // calcul de la deuxieme solution I = I04 - (U * r * co); J = J04 - (U * r * si); -#if (DEBUG_LEVEL2) - { - std::cout << "I2 " << I.t() << std::endl; - std::cout << "J2 " << J.t() << std::endl; - } -#endif + calculSolutionDementhon(I, J, cMo2); } int vpPose::calculArbreDementhon(vpMatrix &Ap, vpColVector &U, vpHomogeneousMatrix &cMo) { -#if (DEBUG_LEVEL1) - std::cout << "begin vpPose::CalculArbreDementhon() " << std::endl; -#endif - int erreur = 0; // test if all points are in front of the camera + const unsigned int idX = 0, idY = 1, idZ = 2, idTranslation = 3; for (unsigned int i = 0; i < npt; ++i) { double z; - z = (cMo[2][0] * c3d[i].get_oX()) + (cMo[2][1] * c3d[i].get_oY()) + (cMo[2][2] * c3d[i].get_oZ()) + cMo[2][3]; + z = (cMo[idZ][idX] * c3d[i].get_oX()) + (cMo[idZ][idY] * c3d[i].get_oY()) + (cMo[idZ][idZ] * c3d[i].get_oZ()) + cMo[idZ][idTranslation]; if (z <= 0.0) { erreur = -1; return erreur; @@ -331,13 +292,14 @@ int vpPose::calculArbreDementhon(vpMatrix &Ap, vpColVector &U, vpHomogeneousMatr vpColVector xprim(npt), yprim(npt); for (unsigned int i = 0; i < npt; ++i) { double eps = - ((cMo[2][0] * c3d[i].get_oX()) + (cMo[2][1] * c3d[i].get_oY()) + (cMo[2][2] * c3d[i].get_oZ())) / cMo[2][3]; + ((cMo[idZ][idX] * c3d[i].get_oX()) + (cMo[idZ][idY] * c3d[i].get_oY()) + (cMo[idZ][idZ] * c3d[i].get_oZ())) / cMo[idZ][idTranslation]; xprim[i] = (1.0 + eps) * c3d[i].get_x(); yprim[i] = (1.0 + eps) * c3d[i].get_y(); } - vpColVector I04(4), J04(4); + const unsigned int sizeHomogeneous = 4; + vpColVector I04(sizeHomogeneous), J04(sizeHomogeneous); I04 = Ap * xprim; J04 = Ap * yprim; @@ -348,11 +310,11 @@ int vpPose::calculArbreDementhon(vpMatrix &Ap, vpColVector &U, vpHomogeneousMatr int erreur2 = 0; for (unsigned int i = 0; i < npt; ++i) { double z; - z = (cMo1[2][0] * c3d[i].get_oX()) + (cMo1[2][1] * c3d[i].get_oY()) + (cMo1[2][2] * c3d[i].get_oZ()) + cMo1[2][3]; + z = (cMo1[idZ][idX] * c3d[i].get_oX()) + (cMo1[idZ][idY] * c3d[i].get_oY()) + (cMo1[idZ][idZ] * c3d[i].get_oZ()) + cMo1[idZ][idTranslation]; if (z <= 0.0) { erreur1 = -1; } - z = (cMo2[2][0] * c3d[i].get_oX()) + (cMo2[2][1] * c3d[i].get_oY()) + (cMo2[2][2] * c3d[i].get_oZ()) + cMo2[2][3]; + z = (cMo2[idZ][idX] * c3d[i].get_oX()) + (cMo2[idZ][idY] * c3d[i].get_oY()) + (cMo2[idZ][idZ] * c3d[i].get_oZ()) + cMo2[idZ][idTranslation]; if (z <= 0.0) { erreur2 = -1; } @@ -360,9 +322,6 @@ int vpPose::calculArbreDementhon(vpMatrix &Ap, vpColVector &U, vpHomogeneousMatr if ((erreur1 == -1) && (erreur2 == -1)) { cMo = cMo_old; -#if (DEBUG_LEVEL3) - std::cout << " End of loop since z < 0 for both solutions" << std::endl; -#endif break; // outside of while due to z < 0 } if ((erreur1 == 0) && (erreur2 == -1)) { @@ -386,67 +345,37 @@ int vpPose::calculArbreDementhon(vpMatrix &Ap, vpColVector &U, vpHomogeneousMatr } } -#if (DEBUG_LEVEL3) - if (erreur1 == 0) { - double s = sqrt(computeResidualDementhon(cMo1) / npt); - vpThetaUVector erc; - cMo1.extract(erc); - std::cout << "it = " << cpt << " cMo1 : residu: " << s << " Theta U rotation: " << vpMath::deg(erc[0]) << " " - << vpMath::deg(erc[1]) << " " << vpMath::deg(erc[2]) << std::endl; - } - else - std::cout << "Pb z < 0 with cMo1" << std::endl; - - if (erreur2 == 0) { - double s = sqrt(computeResidualDementhon(cMo2) / npt); - vpThetaUVector erc; - cMo2.extract(erc); - std::cout << "it = " << cpt << " cMo2 : residu: " << s << " Theta U rotation: " << vpMath::deg(erc[0]) << " " - << vpMath::deg(erc[1]) << " " << vpMath::deg(erc[2]) << std::endl; - } - else - std::cout << "Pb z < 0 with cMo2" << std::endl; -#endif - if (res_min > res_old) { -#if (DEBUG_LEVEL3) - std::cout << "Divergence : res_min = " << res_min << " res_old = " << res_old << std::endl; -#endif cMo = cMo_old; } ++cpt; } /* end of while */ -#if (DEBUG_LEVEL1) - std::cout << "end vpPose::CalculArbreDementhon() return " << erreur << std::endl; -#endif - return erreur; } void vpPose::poseDementhonPlan(vpHomogeneousMatrix &cMo) { -#if (DEBUG_LEVEL1) - std::cout << "begin CCalculPose::PoseDementhonPlan()" << std::endl; -#endif const double svdFactorUsedWhenFailure = 10.; // Factor by which is multipled m_dementhonSvThresh each time the svdDecomposition fails const double svdThresholdLimit = 1e-2; // The svd decomposition will be tested with a threshold up to this value. If with this threshold, the rank of A is still !=3, an exception is thrown const double lnOfSvdFactorUsed = std::log(svdFactorUsedWhenFailure); const double logNOfSvdThresholdLimit = std::log(svdThresholdLimit)/lnOfSvdFactorUsed; vpPoint P; - double cdg[3]; + const unsigned int idX = 0, idY = 1, idZ = 2, size3Dpt = 3; + double cdg[size3Dpt]; /* compute the cog of the 3D points */ - cdg[0] = 0.0; - cdg[1] = 0.0; - cdg[2] = 0.0; + cdg[idX] = 0.0; + cdg[idY] = 0.0; + cdg[idZ] = 0.0; std::list::const_iterator listp_end = listP.end(); for (std::list::const_iterator it = listP.begin(); it != listp_end; ++it) { P = (*it); - cdg[0] += P.get_oX(); - cdg[1] += P.get_oY(); - cdg[2] += P.get_oZ(); + cdg[idX] += P.get_oX(); + cdg[idY] += P.get_oY(); + cdg[idZ] += P.get_oZ(); } - for (unsigned int i = 0; i < 3; ++i) { + + for (unsigned int i = 0; i < size3Dpt; ++i) { cdg[i] /= npt; } // --comment: print cdg cdg of 0 of 1 and of 2 @@ -462,13 +391,14 @@ void vpPose::poseDementhonPlan(vpHomogeneousMatrix &cMo) c3d.push_back(P); } - vpMatrix A(npt, 4); + const unsigned int sizeHomogeneous = 4, idHomogeneous = 3; + vpMatrix A(npt, sizeHomogeneous); for (unsigned int i = 0; i < npt; ++i) { - A[i][0] = c3d[i].get_oX(); - A[i][1] = c3d[i].get_oY(); - A[i][2] = c3d[i].get_oZ(); - A[i][3] = 1.0; + A[i][idX] = c3d[i].get_oX(); + A[i][idY] = c3d[i].get_oY(); + A[i][idZ] = c3d[i].get_oZ(); + A[i][idHomogeneous] = 1.0; } vpColVector sv; vpMatrix Ap, imA, imAt, kAt; @@ -480,9 +410,10 @@ void vpPose::poseDementhonPlan(vpHomogeneousMatrix &cMo) double svdThreshold = m_dementhonSvThresh; int irank = 0; int i = 0; + const unsigned int expectedRank = 3; while ((i < nbMaxIter) && (!isRankEqualTo3)) { irank = A.pseudoInverse(Ap, sv, svdThreshold, imA, imAt, kAt); - if (irank == 3) { + if (irank == expectedRank) { isRankEqualTo3 = true; } else { @@ -504,17 +435,10 @@ void vpPose::poseDementhonPlan(vpHomogeneousMatrix &cMo) throw(vpException(vpException::fatalError, errorMsg.str())); } // calcul de U - vpColVector U(4); - for (unsigned int i = 0; i < 4; ++i) { + vpColVector U(sizeHomogeneous); + for (unsigned int i = 0; i < sizeHomogeneous; ++i) { U[i] = kAt[0][i]; } -#if (DEBUG_LEVEL2) - { - std::cout << "A" << std::endl << A << std::endl; - std::cout << "A^+" << std::endl << Ap << std::endl; - std::cout << "U^T = " << U.t() << std::endl; - } -#endif vpColVector xi(npt); vpColVector yi(npt); @@ -524,25 +448,13 @@ void vpPose::poseDementhonPlan(vpHomogeneousMatrix &cMo) yi[i] = c3d[i].get_y(); } - vpColVector I04(4), J04(4); + vpColVector I04(sizeHomogeneous), J04(sizeHomogeneous); I04 = Ap * xi; J04 = Ap * yi; vpHomogeneousMatrix cMo1, cMo2; calculTwoSolutionsDementhonPlan(I04, J04, U, cMo1, cMo2); -#if DEBUG_LEVEL3 - double res = sqrt(computeResidualDementhon(cMo1) / npt); - vpThetaUVector erc; - cMo1.extract(erc); - std::cout << "cMo Start Tree 1 : res " << res << " Theta U rotation: " << vpMath::deg(erc[0]) << " " - << vpMath::deg(erc[1]) << " " << vpMath::deg(erc[2]) << std::endl; - res = sqrt(computeResidualDementhon(cMo2) / npt); - cMo2.extract(erc); - std::cout << "cMo Start Tree 2 : res " << res << " Theta U rotation: " << vpMath::deg(erc[0]) << " " - << vpMath::deg(erc[1]) << " " << vpMath::deg(erc[2]) << std::endl; -#endif - int erreur1 = calculArbreDementhon(Ap, U, cMo1); int erreur2 = calculArbreDementhon(Ap, U, cMo2); @@ -566,26 +478,11 @@ void vpPose::poseDementhonPlan(vpHomogeneousMatrix &cMo) else { cMo = cMo2; } - -#if DEBUG_LEVEL3 - if (erreur1 == -1) - std::cout << "Pb z < 0 with Start Tree 1" << std::endl; - if (erreur2 == -1) - std::cout << "Pb z < 0 with Start Tree 2" << std::endl; - if (s1 <= s2) - std::cout << " Tree 1 chosen " << std::endl; - else - std::cout << " Tree 2 chosen " << std::endl; -#endif } - - cMo[0][3] -= ((cdg[0] * cMo[0][0]) + (cdg[1] * cMo[0][1]) + (cdg[2] * cMo[0][2])); - cMo[1][3] -= ((cdg[0] * cMo[1][0]) + (cdg[1] * cMo[1][1]) + (cdg[2] * cMo[1][2])); - cMo[2][3] -= ((cdg[0] * cMo[2][0]) + (cdg[1] * cMo[2][1]) + (cdg[2] * cMo[2][2])); - -#if (DEBUG_LEVEL1) - std::cout << "end CCalculPose::PoseDementhonPlan()" << std::endl; -#endif + const unsigned int idTranslation = 3; + 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])); } double vpPose::computeResidualDementhon(const vpHomogeneousMatrix &cMo) @@ -595,12 +492,12 @@ double vpPose::computeResidualDementhon(const vpHomogeneousMatrix &cMo) // Be careful: since c3d has been translated so that point0 is at the cdg, // cMo here corresponds to object frame at that point, i.e, only the one used // internally to Dementhon's method - + const unsigned int idX = 0, idY = 1, idZ = 2, idTranslation = 3; for (unsigned int i = 0; i < npt; ++i) { - double X = (c3d[i].get_oX() * cMo[0][0]) + (c3d[i].get_oY() * cMo[0][1]) + (c3d[i].get_oZ() * cMo[0][2]) + cMo[0][3]; - double Y = (c3d[i].get_oX() * cMo[1][0]) + (c3d[i].get_oY() * cMo[1][1]) + (c3d[i].get_oZ() * cMo[1][2]) + cMo[1][3]; - double Z = (c3d[i].get_oX() * cMo[2][0]) + (c3d[i].get_oY() * cMo[2][1]) + (c3d[i].get_oZ() * cMo[2][2]) + cMo[2][3]; + double X = (c3d[i].get_oX() * cMo[idX][idX]) + (c3d[i].get_oY() * cMo[idX][idY]) + (c3d[i].get_oZ() * cMo[idX][idZ]) + cMo[idX][idTranslation]; + double Y = (c3d[i].get_oX() * cMo[idY][idX]) + (c3d[i].get_oY() * cMo[idY][idY]) + (c3d[i].get_oZ() * cMo[idY][idZ]) + cMo[idY][idTranslation]; + double Z = (c3d[i].get_oX() * cMo[idZ][idX]) + (c3d[i].get_oY() * cMo[idZ][idY]) + (c3d[i].get_oZ() * cMo[idZ][idZ]) + cMo[idZ][idTranslation]; double x = X / Z; double y = Y / Z; @@ -610,11 +507,8 @@ double vpPose::computeResidualDementhon(const vpHomogeneousMatrix &cMo) return squared_error; } +END_VISP_NAMESPACE + #undef EPS_DEM #undef SEUIL_RESIDUAL #undef ITER_MAX -#undef DEBUG_LEVEL1 -#undef DEBUG_LEVEL2 -#undef DEBUG_LEVEL3 - -END_VISP_NAMESPACE diff --git a/modules/vision/src/pose-estimation/vpPoseLagrange.cpp b/modules/vision/src/pose-estimation/vpPoseLagrange.cpp index e497f4dcf2..c8885c9bf6 100644 --- a/modules/vision/src/pose-estimation/vpPoseLagrange.cpp +++ b/modules/vision/src/pose-estimation/vpPoseLagrange.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -35,9 +35,6 @@ BEGIN_VISP_NAMESPACE -#define DEBUG_LEVEL1 0 -#define DEBUG_LEVEL2 0 - /**********************************************************************/ /* FONCTION : CalculTranslation */ /* ROLE : Calcul de la translation entre la */ @@ -48,75 +45,49 @@ BEGIN_VISP_NAMESPACE static void calculTranslation(vpMatrix &a, vpMatrix &b, unsigned int nl, unsigned int nc1, unsigned int nc3, vpColVector &x1, vpColVector &x2) { - - try { - unsigned int i, j; - - vpMatrix ct(3, nl); - for (i = 0; i < 3; ++i) { - for (j = 0; j < nl; ++j) { - ct[i][j] = b[j][i + nc3]; - } + unsigned int i, j; + const unsigned int nbRows = 3; + vpMatrix ct(nbRows, nl); + for (i = 0; i < nbRows; ++i) { + for (j = 0; j < nl; ++j) { + ct[i][j] = b[j][i + nc3]; } + } - vpMatrix c; - c = ct.t(); + vpMatrix c; + c = ct.t(); - vpMatrix ctc; - ctc = ct * c; + vpMatrix ctc; + ctc = ct * c; - vpMatrix ctc1; // (C^T C)^(-1) - ctc1 = ctc.inverseByLU(); + vpMatrix ctc1; // (C^T C)^(-1) + ctc1 = ctc.inverseByLU(); - vpMatrix cta; - vpMatrix ctb; - cta = ct * a; /* C^T A */ - ctb = ct * b; /* C^T B */ - -#if (DEBUG_LEVEL2) - { - std::cout << "ctc " << std::endl << ctc; - std::cout << "cta " << std::endl << cta; - std::cout << "ctb " << std::endl << ctb; - } -#endif - - vpColVector X2(nc3); - vpMatrix CTB(nc1, nc3); - for (i = 0; i < nc1; ++i) { - for (j = 0; j < nc3; ++j) { - CTB[i][j] = ctb[i][j]; - } - } + vpMatrix cta; + vpMatrix ctb; + cta = ct * a; /* C^T A */ + ctb = ct * b; /* C^T B */ + vpColVector X2(nc3); + vpMatrix CTB(nc1, nc3); + for (i = 0; i < nc1; ++i) { for (j = 0; j < nc3; ++j) { - X2[j] = x2[j]; + CTB[i][j] = ctb[i][j]; } + } - vpColVector sv; // C^T A X1 + C^T B X2) - sv = (cta * x1) + (CTB * X2); // C^T A X1 + C^T B X2) - -#if (DEBUG_LEVEL2) - std::cout << "sv " << sv.t(); -#endif - - vpColVector X3; /* X3 = - (C^T C )^{-1} C^T (A X1 + B X2) */ - X3 = -ctc1 * sv; + for (j = 0; j < nc3; ++j) { + X2[j] = x2[j]; + } -#if (DEBUG_LEVEL2) - std::cout << "x3 " << X3.t(); -#endif + vpColVector sv; // C^T A X1 + C^T B X2) + sv = (cta * x1) + (CTB * X2); // C^T A X1 + C^T B X2) - for (i = 0; i < nc1; ++i) { - x2[i + nc3] = X3[i]; - } - } - catch (...) { + vpColVector X3; /* X3 = - (C^T C )^{-1} C^T (A X1 + B X2) */ + X3 = -ctc1 * sv; - // en fait il y a des dizaines de raisons qui font que cette fonction - // rende une erreur (matrice pas inversible, pb de memoire etc...) - vpERROR_TRACE(" "); - throw; + for (i = 0; i < nc1; ++i) { + x2[i + nc3] = X3[i]; } } @@ -127,126 +98,88 @@ static void calculTranslation(vpMatrix &a, vpMatrix &b, unsigned int nl, unsigne // sous la contrainte || x1 || = 1 // ou A est de dimension nl x nc1 et B nl x nc2 //********************************************************************* - -// --comment: define EPS at 1.e-5 - static void lagrange(vpMatrix &a, vpMatrix &b, vpColVector &x1, vpColVector &x2) { -#if (DEBUG_LEVEL1) - std::cout << "begin (CLagrange.cc)Lagrange(...) " << std::endl; -#endif - - try { - unsigned int i, imin; - - vpMatrix ata; // A^T A - ata = a.t() * a; - vpMatrix btb; // B^T B - btb = b.t() * b; - - vpMatrix bta; // B^T A - bta = b.t() * a; - - vpMatrix btb1; // (B^T B)^(-1) - - /* Warning: - when using btb.inverseByLU() that call cv::inv(cv::DECOMP_LU) with - OpenCV 3.1.0 and 3.2.0 we notice that OpenCV is not able to compute the - inverse of the following matrix: - - btb[9,9]= - 0.015925 0.0 0.0030866 0.00035 0.0 0.000041 0.105 - 0.0 0.0346242 0.0 0.015925 -0.0050979 0.0 0.00035 - -0.000063 0.0 0.105 -0.0637464 0.0030866 -0.0050979 0.0032301 - 0.000041 -0.000063 0.000016 0.0346242 -0.0637464 0.0311185 0.00035 - 0.0 0.000041 0.0001 0.0 0.000012 0.01 0.0 - 0.0011594 0.0 0.00035 -0.000063 0.0 0.0001 -0.000018 - 0.0 0.01 -0.0018040 0.000041 -0.000063 0.000016 0.000012 - -0.000018 0.000005 0.0011594 -0.0018040 0.0004599 0.105 0.0 - 0.0346242 0.01 0.0 0.0011594 5.0 0.0 0.13287 - 0.0 0.105 -0.0637464 0.0 0.01 -0.0018040 0.0 - 5.0 -0.731499 0.0346242 -0.0637464 0.0311185 0.0011594 -0.0018040 - 0.0004599 0.13287 -0.731499 0.454006 - - That's why instead of using inverseByLU() we are now using pseudoInverse() - */ + unsigned int i, imin; + + vpMatrix ata; // A^T A + ata = a.t() * a; + vpMatrix btb; // B^T B + btb = b.t() * b; + + vpMatrix bta; // B^T A + bta = b.t() * a; + + vpMatrix btb1; // (B^T B)^(-1) + + /* Warning: + when using btb.inverseByLU() that call cv::inv(cv::DECOMP_LU) with + OpenCV 3.1.0 and 3.2.0 we notice that OpenCV is not able to compute the + inverse of the following matrix: + + btb[9,9]= + 0.015925 0.0 0.0030866 0.00035 0.0 0.000041 0.105 + 0.0 0.0346242 0.0 0.015925 -0.0050979 0.0 0.00035 + -0.000063 0.0 0.105 -0.0637464 0.0030866 -0.0050979 0.0032301 + 0.000041 -0.000063 0.000016 0.0346242 -0.0637464 0.0311185 0.00035 + 0.0 0.000041 0.0001 0.0 0.000012 0.01 0.0 + 0.0011594 0.0 0.00035 -0.000063 0.0 0.0001 -0.000018 + 0.0 0.01 -0.0018040 0.000041 -0.000063 0.000016 0.000012 + -0.000018 0.000005 0.0011594 -0.0018040 0.0004599 0.105 0.0 + 0.0346242 0.01 0.0 0.0011594 5.0 0.0 0.13287 + 0.0 0.105 -0.0637464 0.0 0.01 -0.0018040 0.0 + 5.0 -0.731499 0.0346242 -0.0637464 0.0311185 0.0011594 -0.0018040 + 0.0004599 0.13287 -0.731499 0.454006 + + That's why instead of using inverseByLU() we are now using pseudoInverse() + */ #if 0 - if (b.getRows() >= b.getCols()) { - btb1 = btb.inverseByLU(); - } - else { - btb1 = btb.pseudoInverse(); - } -#else + if (b.getRows() >= b.getCols()) { + btb1 = btb.inverseByLU(); + } + else { btb1 = btb.pseudoInverse(); + } +#else + btb1 = btb.pseudoInverse(); #endif -#if (DEBUG_LEVEL1) - { - std::cout << " BTB1 * BTB : " << std::endl << btb1 * btb << std::endl; - std::cout << " BTB * BTB1 : " << std::endl << btb * btb1 << std::endl; - } -#endif - - vpMatrix r; // (B^T B)^(-1) B^T A - r = btb1 * bta; - - vpMatrix e; // - A^T B (B^T B)^(-1) B^T A - e = -(a.t() * b) * r; - - e += ata; // calcul E = A^T A - A^T B (B^T B)^(-1) B^T A - -#if (DEBUG_LEVEL1) - { - std::cout << " E :" << std::endl << e << std::endl; - } -#endif - - e.svd(x1, ata); // destructif sur e - // calcul du vecteur propre de E correspondant a la valeur propre min. - imin = 0; + vpMatrix r; // (B^T B)^(-1) B^T A + r = btb1 * bta; - unsigned int v_x1_rows = x1.getRows(); - for (i = 0; i < v_x1_rows; ++i) { - if (x1[i] < x1[imin]) { - imin = i; - } - } + vpMatrix e; // - A^T B (B^T B)^(-1) B^T A + e = -(a.t() * b) * r; -#if (DEBUG_LEVEL1) - { - printf("SV(E) : %.15lf %.15lf %.15lf\n", x1[0], x1[1], x1[2]); - std::cout << " i_min " << imin << std::endl; - } -#endif - unsigned int x1_rows = x1.getRows(); - for (i = 0; i < x1_rows; ++i) { - x1[i] = ata[i][imin]; - } + e += ata; // calcul E = A^T A - A^T B (B^T B)^(-1) B^T A - x2 = -(r * x1); // X_2 = - (B^T B)^(-1) B^T A X_1 + e.svd(x1, ata); // destructif sur e + // calcul du vecteur propre de E correspondant a la valeur propre min. + imin = 0; -#if (DEBUG_LEVEL1) - { - std::cout << " X1 : " << x1.t() << std::endl; - std::cout << " V : " << std::endl << ata << std::endl; + unsigned int v_x1_rows = x1.getRows(); + for (i = 0; i < v_x1_rows; ++i) { + if (x1[i] < x1[imin]) { + imin = i; } -#endif } - catch (...) { - vpERROR_TRACE(" "); - throw; + + unsigned int x1_rows = x1.getRows(); + for (i = 0; i < x1_rows; ++i) { + x1[i] = ata[i][imin]; } -#if (DEBUG_LEVEL1) - std::cout << "end (CLagrange.cc)Lagrange(...) " << std::endl; -#endif + + x2 = -(r * x1); // X_2 = - (B^T B)^(-1) B^T A X_1 } void vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, bool *p_isPlan, double *p_a, double *p_b, double *p_c, double *p_d) { -#if (DEBUG_LEVEL1) - std::cout << "begin vpPose::PoseLagrangePlan(...) " << std::endl; -#endif + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + const unsigned int index_5 = 5; + // determination of the plane equation a X + b Y + c Z + d = 0 double a, b, c, d; @@ -275,7 +208,7 @@ void vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, bool *p_isPlan, double * } } - if (c < 0.0) { // imposing c >= 0 + if (c < 0.0) { // imposing c greater than or equal to 0 a = -a; b = -b; c = -c; @@ -287,273 +220,225 @@ void vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, bool *p_isPlan, double * b *= n; c *= n; d *= n; - // printf("a = %lf, b = %lf, c = %lf, d = %lf\n",a,b,c,d); // transformation to have object plane with equation Z = 0 - vpColVector r1(3), r2(3), r3(3); + const unsigned int size = 3; + vpColVector r1(size), r2(size), r3(size); - r3[0] = a; - r3[1] = b; - r3[2] = c; + r3[index_0] = a; + r3[index_1] = b; + r3[index_2] = c; // build r1 as a unit vector orthogonal to r3 double n1 = sqrt(1.0 - (a * a)); double n2 = sqrt(1.0 - (b * b)); if (n1 >= n2) { - r1[0] = n1; - r1[1] = (-a * b) / n1; - r1[2] = (-a * c) / n1; + r1[index_0] = n1; + r1[index_1] = (-a * b) / n1; + r1[index_2] = (-a * c) / n1; } else { - r1[0] = (-a * b) / n2; - r1[1] = n2; - r1[2] = (-b * c) / n2; + r1[index_0] = (-a * b) / n2; + r1[index_1] = n2; + r1[index_2] = (-b * c) / n2; } r2 = vpColVector::crossProd(r3, r1); vpHomogeneousMatrix fMo; - for (unsigned int i = 0; i < 3; ++i) { - fMo[0][i] = r1[i]; - fMo[1][i] = r2[i]; - fMo[2][i] = r3[i]; + const unsigned int sizeRotation = 3; + const unsigned int idX = 0, idY = 1, idZ = 2, idTranslation = 3; + for (unsigned int i = 0; i < sizeRotation; ++i) { + fMo[idX][i] = r1[i]; + fMo[idY][i] = r2[i]; + fMo[idZ][i] = r3[i]; } - fMo[0][3] = 0.0; - fMo[1][3] = 0.0; - fMo[2][3] = d; + fMo[idX][idTranslation] = 0.0; + fMo[idY][idTranslation] = 0.0; + fMo[idZ][idTranslation] = d; - // std::cout << "fMo : " << std::endl << fMo << std::endl; // Build and solve the system unsigned int k = 0; unsigned int nl = npt * 2; - vpMatrix A(nl, 3); - vpMatrix B(nl, 6); + const unsigned int nbColsA = 3, nbColsB = 6; + 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; for (std::list::const_iterator it = listP.begin(); it != listp_end; ++it) { P = *it; // Transform each point in plane Z = 0 - vpColVector Xf, X(4); - X[0] = P.get_oX(); - X[1] = P.get_oY(); - X[2] = P.get_oZ(); - X[3] = 1.0; + vpColVector Xf, X(sizeHomogeneous); + X[idX] = P.get_oX(); + X[idY] = P.get_oY(); + X[idZ] = P.get_oZ(); + X[idHomogeneous] = 1.0; Xf = fMo * X; - // printf("Z = 0 = %lf\n",Xf[2]); // build the system - A[k][0] = -Xf[0]; - A[k][1] = 0.0; - A[k][2] = Xf[0] * P.get_x(); - - A[k + 1][0] = 0.0; - A[k + 1][1] = -Xf[0]; - A[k + 1][2] = Xf[0] * P.get_y(); - - B[k][0] = -Xf[1]; - B[k][1] = 0.0; - B[k][2] = Xf[1] * P.get_x(); - B[k][3] = -1.0; - B[k][4] = 0.0; - B[k][5] = P.get_x(); - - B[k + 1][0] = 0.0; - B[k + 1][1] = -Xf[1]; - B[k + 1][2] = Xf[1] * P.get_y(); - B[k + 1][3] = 0.0; - B[k + 1][4] = -1.0; - B[k + 1][5] = P.get_y(); + A[k][index_0] = -Xf[0]; + A[k][index_1] = 0.0; + A[k][index_2] = Xf[0] * P.get_x(); + + A[k + 1][index_0] = 0.0; + A[k + 1][index_1] = -Xf[0]; + A[k + 1][index_2] = Xf[0] * P.get_y(); + + B[k][index_0] = -Xf[1]; + B[k][index_1] = 0.0; + B[k][index_2] = Xf[1] * P.get_x(); + B[k][index_3] = -1.0; + B[k][index_4] = 0.0; + B[k][index_5] = P.get_x(); + + B[k + 1][index_0] = 0.0; + B[k + 1][index_1] = -Xf[1]; + B[k + 1][index_2] = Xf[1] * P.get_y(); + B[k + 1][index_3] = 0.0; + B[k + 1][index_4] = -1.0; + B[k + 1][index_5] = P.get_y(); k += 2; } - vpColVector X1(3); - vpColVector X2(6); - -#if (DEBUG_LEVEL2) - { - std::cout << "A " << std::endl << A << std::endl; - std::cout << "B " << std::endl << B << std::endl; - } -#endif + 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); + vpColVector X2(sizeX2); lagrange(A, B, X1, X2); -#if (DEBUG_LEVEL2) - { - std::cout << "A X1+B X2 (should be 0): " << (A * X1 + B * X2).t() << std::endl; - std::cout << " X1 norm: " << X1.sumSquare() << std::endl; - } -#endif - - if (X2[5] < 0.0) { /* to obtain Zo > 0 */ - for (unsigned int i = 0; i < 3; ++i) { + if (X2[lastX2] < 0.0) { /* to obtain Zo > 0 */ + for (unsigned int i = 0; i < sizeX1; ++i) { X1[i] = -X1[i]; } - for (unsigned int i = 0; i < 6; ++i) { + + for (unsigned int i = 0; i < sizeX2; ++i) { X2[i] = -X2[i]; } } double s = 0.0; - for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int i = 0; i < sizeX1; ++i) { s += (X1[i] * X2[i]); } - for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int i = 0; i < sizeX1; ++i) { X2[i] -= (s * X1[i]); } /* X1^T X2 = 0 */ // --comment: s equals 0.0 - s = (X2[0] * X2[0]) + (X2[1] * X2[1]) + (X2[2] * X2[2]); // To avoid a Coverity copy/past error + s = (X2[index_0] * X2[index_0]) + (X2[index_1] * X2[index_1]) + (X2[index_2] * X2[index_2]); // To avoid a Coverity copy/past error if (s < 1e-10) { - /* - // std::cout << "Points that produce an error: " << std::endl; - // for (std::list::const_iterator it = listP.begin(); it - // != listP.end(); ++it) - // { - // std::cout << "P: " << (*it).get_x() << " " << (*it).get_y() << - // " " - // << (*it).get_oX() << " " << (*it).get_oY() << " " << - // (*it).get_oZ() << std::endl; - // } - */ throw(vpException(vpException::divideByZeroError, "Division by zero in Lagrange pose computation " "(planar plane case)")); } s = 1.0 / sqrt(s); - for (unsigned int i = 0; i < 3; ++i) { + const unsigned int val_3 = 3, nc1 = 3, nc3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { X2[i] *= s; - } /* X2^T X2 = 1 */ + } /* X2^T X2 is equal to 1 */ - calculTranslation(A, B, nl, 3, 3, X1, X2); + calculTranslation(A, B, nl, nc1, nc3, X1, X2); vpHomogeneousMatrix cMf; /* X1 x X2 */ - cMf[0][2] = (X1[1] * X2[2]) - (X1[2] * X2[1]); - cMf[1][2] = (X1[2] * X2[0]) - (X1[0] * X2[2]); - cMf[2][2] = (X1[0] * X2[1]) - (X1[1] * X2[0]); + cMf[index_0][index_2] = (X1[index_1] * X2[index_2]) - (X1[index_2] * X2[index_1]); + cMf[index_1][index_2] = (X1[index_2] * X2[index_0]) - (X1[index_0] * X2[index_2]); + cMf[index_2][index_2] = (X1[index_0] * X2[index_1]) - (X1[index_1] * X2[index_0]); /* calcul de la matrice de passage */ - for (unsigned int i = 0; i < 3; ++i) { - cMf[i][0] = X1[i]; - cMf[i][1] = X2[i]; - cMf[i][3] = X2[i + 3]; + 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]; } // Apply the transform to go back to object frame cMo = cMf * fMo; - -#if (DEBUG_LEVEL1) - std::cout << "end vpCalculPose::PoseLagrangePlan(...) " << std::endl; -#endif } void vpPose::poseLagrangeNonPlan(vpHomogeneousMatrix &cMo) { - -#if (DEBUG_LEVEL1) - std::cout << "begin CPose::PoseLagrangeNonPlan(...) " << std::endl; -#endif try { double s; unsigned int i; unsigned int k = 0; - unsigned int nl = npt * 2; + const unsigned int twice = 2; + unsigned int nl = npt * twice; + const unsigned int npt_min = 6; - if (npt < 6) { + if (npt < npt_min) { throw(vpException(vpException::dimensionError, "Lagrange, non planar case, insufficient number of points %d < 6\n", npt)); } - vpMatrix a(nl, 3); - vpMatrix b(nl, 9); + const unsigned int nbColsA = 3, nbColsB = 9; + vpMatrix a(nl, nbColsA); + vpMatrix b(nl, nbColsB); b = 0; vpPoint P; i = 0; std::list::const_iterator listp_end = listP.end(); + 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; for (std::list::const_iterator it = listP.begin(); it != listp_end; ++it) { P = *it; - a[k][0] = -P.get_oX(); - a[k][1] = 0.0; - a[k][2] = P.get_oX() * P.get_x(); + a[k][id0] = -P.get_oX(); + a[k][id1] = 0.0; + a[k][id2] = P.get_oX() * P.get_x(); - a[k + 1][0] = 0.0; - a[k + 1][1] = -P.get_oX(); - a[k + 1][2] = P.get_oX() * P.get_y(); + a[k + 1][id0] = 0.0; + a[k + 1][id1] = -P.get_oX(); + a[k + 1][id2] = P.get_oX() * P.get_y(); - b[k][0] = -P.get_oY(); - b[k][1] = 0.0; - b[k][2] = P.get_oY() * P.get_x(); + b[k][id0] = -P.get_oY(); + b[k][id1] = 0.0; + b[k][id2] = P.get_oY() * P.get_x(); - b[k][3] = -P.get_oZ(); - b[k][4] = 0.0; - b[k][5] = P.get_oZ() * P.get_x(); + b[k][id3] = -P.get_oZ(); + b[k][id4] = 0.0; + b[k][id5] = P.get_oZ() * P.get_x(); - b[k][6] = -1.0; - b[k][7] = 0.0; - b[k][8] = P.get_x(); + b[k][id6] = -1.0; + b[k][id7] = 0.0; + b[k][id8] = P.get_x(); - b[k + 1][0] = 0.0; - b[k + 1][1] = -P.get_oY(); - b[k + 1][2] = P.get_oY() * P.get_y(); + b[k + 1][id0] = 0.0; + b[k + 1][id1] = -P.get_oY(); + b[k + 1][id2] = P.get_oY() * P.get_y(); - b[k + 1][3] = 0.0; - b[k + 1][4] = -P.get_oZ(); - b[k + 1][5] = P.get_oZ() * P.get_y(); + b[k + 1][id3] = 0.0; + b[k + 1][id4] = -P.get_oZ(); + b[k + 1][id5] = P.get_oZ() * P.get_y(); - b[k + 1][6] = 0.0; - b[k + 1][7] = -1.0; - b[k + 1][8] = P.get_y(); + b[k + 1][id6] = 0.0; + b[k + 1][id7] = -1.0; + b[k + 1][id8] = P.get_y(); k += 2; } - vpColVector X1(3); - vpColVector X2(9); - -#if (DEBUG_LEVEL2) - { - std::cout << "a " << a << std::endl; - std::cout << "b " << b << std::endl; - } -#endif + vpColVector X1(nbColsA); // X1 is of size A^T A + vpColVector X2(nbColsB); // X2 is of size B^T B lagrange(a, b, X1, X2); -#if (DEBUG_LEVEL2) - { - std::cout << "ax1+bx2 (devrait etre 0) " << (a * X1 + b * X2).t() << std::endl; - std::cout << "norme X1 " << X1.sumSquare() << std::endl; - } -#endif - - if (X2[8] < 0.0) { /* car Zo > 0 */ + if (X2[id8] < 0.0) { /* because Zo greater than 0 */ X1 *= -1; X2 *= -1; } s = 0.0; - for (i = 0; i < 3; ++i) { + for (i = 0; i < nbColsA; ++i) { s += (X1[i] * X2[i]); } - for (i = 0; i < 3; ++i) { + for (i = 0; i < nbColsA; ++i) { X2[i] -= (s * X1[i]); - } /* X1^T X2 = 0 */ + } /* X1^T X2 is null */ - s = (X2[0] * X2[0]) + (X2[1] * X2[1]) + (X2[2] * X2[2]); // To avoid a Coverity copy/past error + s = (X2[id0] * X2[id0]) + (X2[id1] * X2[id1]) + (X2[id2] * X2[id2]); // To avoid a Coverity copy/past error if (s < 1e-10) { - /* - // std::cout << "Points that produce an error: " << std::endl; - // for (std::list::const_iterator it = listP.begin(); it - // != listP.end(); ++it) - // { - // std::cout << "P: " << (*it).get_x() << " " << (*it).get_y() << - // " " - // << (*it).get_oX() << " " << (*it).get_oY() << " " << - // (*it).get_oZ() << std::endl; - // } - // vpERROR_TRACE(" division par zero " ) ; - */ throw(vpException(vpException::divideByZeroError, "Division by zero in Lagrange pose computation (non " "planar plane case)")); } @@ -563,30 +448,24 @@ void vpPose::poseLagrangeNonPlan(vpHomogeneousMatrix &cMo) X2[i] *= s; } /* X2^T X2 = 1 */ - X2[3] = (X1[1] * X2[2]) - (X1[2] * X2[1]); - X2[4] = (X1[2] * X2[0]) - (X1[0] * X2[2]); - X2[5] = (X1[0] * X2[1]) - (X1[1] * X2[0]); + X2[id3] = (X1[id1] * X2[id2]) - (X1[id2] * X2[id1]); + X2[id4] = (X1[id2] * X2[id0]) - (X1[id0] * X2[id2]); + X2[id5] = (X1[id0] * X2[id1]) - (X1[id1] * X2[id0]); - calculTranslation(a, b, nl, 3, 6, X1, X2); + const unsigned int nc1 = 3, nc3 = 6; + calculTranslation(a, b, nl, nc1, nc3, X1, X2); for (i = 0; i < 3; ++i) { - cMo[i][0] = X1[i]; - cMo[i][1] = X2[i]; - cMo[i][2] = X2[i + 3]; - cMo[i][3] = X2[i + 6]; + cMo[i][id0] = X1[i]; + cMo[i][id1] = X2[i]; + cMo[i][id2] = X2[i + id3]; + cMo[i][id3] = X2[i + id6]; } } catch (...) { throw; // throw the original exception } - -#if (DEBUG_LEVEL1) - std::cout << "end vpCalculPose::PoseLagrangeNonPlan(...) " << std::endl; -#endif } -#undef DEBUG_LEVEL1 -#undef DEBUG_LEVEL2 - END_VISP_NAMESPACE diff --git a/modules/vision/src/pose-estimation/vpPoseLowe.cpp b/modules/vision/src/pose-estimation/vpPoseLowe.cpp index 16350f1714..f9c59bc110 100644 --- a/modules/vision/src/pose-estimation/vpPoseLowe.cpp +++ b/modules/vision/src/pose-estimation/vpPoseLowe.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -41,20 +41,10 @@ #include "private/vpLevenbergMarquartd.h" #include -BEGIN_VISP_NAMESPACE - #define NBR_PAR 6 #define X3_SIZE 3 #define MINIMUM 0.000001 -#define DEBUG_LEVEL1 0 - -// ------------------------------------------------------------------------ -// FONCTION LOWE : -// ------------------------------------------------------------------------ -// Calcul de la pose pour un objet 3D -// ------------------------------------------------------------------------ - /* * MACRO : MIJ * @@ -76,32 +66,46 @@ BEGIN_VISP_NAMESPACE */ #define MIJ(m, i, j, s) ((m) + ((long)(i) * (long)(s)) + (long)(j)) #define NBPTMAX 50 +#define MINI 0.001 +#define MINIMUM 0.000001 + +BEGIN_VISP_NAMESPACE + +// ------------------------------------------------------------------------ +// FONCTION LOWE : +// ------------------------------------------------------------------------ +// Calcul de la pose pour un objet 3D +// ------------------------------------------------------------------------ // Je hurle d'horreur devant ces variable globale... static double XI[NBPTMAX], YI[NBPTMAX]; static double XO[NBPTMAX], YO[NBPTMAX], ZO[NBPTMAX]; -#define MINI 0.001 -#define MINIMUM 0.000001 - void eval_function(int npt, double *xc, double *f); void fcn(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int iflag); void eval_function(int npt, double *xc, double *f) { int i; - double u[3]; - - u[0] = xc[3]; /* Rx */ - u[1] = xc[4]; /* Ry */ - u[2] = xc[5]; /* Rz */ - - vpRotationMatrix rd(u[0], u[1], u[2]); + const unsigned int sizeU = 3; + double u[sizeU]; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + const unsigned int index_5 = 5; + + u[index_0] = xc[index_3]; /* Rx */ + u[index_1] = xc[index_4]; /* Ry */ + u[index_2] = xc[index_5]; /* Rz */ + + vpRotationMatrix rd(u[index_0], u[index_1], u[index_2]); // --comment: rot_mat(u,rd) matrice de rotation correspondante for (i = 0; i < npt; ++i) { - double x = (rd[0][0] * XO[i]) + (rd[0][1] * YO[i]) + (rd[0][2] * ZO[i]) + xc[0]; - double y = (rd[1][0] * XO[i]) + (rd[1][1] * YO[i]) + (rd[1][2] * ZO[i]) + xc[1]; - double z = (rd[2][0] * XO[i]) + (rd[2][1] * YO[i]) + (rd[2][2] * ZO[i]) + xc[2]; + double x = (rd[index_0][index_0] * XO[i]) + (rd[index_0][index_1] * YO[i]) + (rd[index_0][index_2] * ZO[i]) + xc[index_0]; + double y = (rd[index_1][index_0] * XO[i]) + (rd[index_1][index_1] * YO[i]) + (rd[index_1][index_2] * ZO[i]) + xc[index_1]; + double z = (rd[index_2][index_0] * XO[i]) + (rd[index_2][index_1] * YO[i]) + (rd[index_2][index_2] * ZO[i]) + xc[index_2]; f[i] = (x / z) - XI[i]; f[npt + i] = (y / z) - YI[i]; // --comment: write fi and fi+1 @@ -143,26 +147,34 @@ void fcn(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int i if (m < n) { printf("pas assez de points\n"); } - npt = m / 2; + const int half = 2; + npt = m / half; - if (iflag == 1) { + const int flagFunc = 1, flagJacobian = 2; + if (iflag == flagFunc) { eval_function(npt, xc, fvecc); } - else if (iflag == 2) { + else if (iflag == flagJacobian) { double u1, u2, u3; - u[0] = xc[3]; - u[1] = xc[4]; - u[2] = xc[5]; - - rd.build(u[0], u[1], u[2]); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + const unsigned int index_5 = 5; + u[index_0] = xc[index_3]; + u[index_1] = xc[index_4]; + u[index_2] = xc[index_5]; + + rd.build(u[index_0], u[index_1], u[index_2]); /* a partir de l'axe de rotation, calcul de la matrice de rotation. */ // --comment: rot_mat of u rd - double tt = sqrt((u[0] * u[0]) + (u[1] * u[1]) + (u[2] * u[2])); /* angle de rot */ + double tt = sqrt((u[index_0] * u[index_0]) + (u[index_1] * u[index_1]) + (u[index_2] * u[index_2])); /* angle de rot */ if (tt >= MINIMUM) { - u1 = u[0] / tt; - u2 = u[1] / tt; /* axe de rotation unitaire */ - u3 = u[2] / tt; + u1 = u[index_0] / tt; + u2 = u[index_1] / tt; /* axe de rotation unitaire */ + u3 = u[index_2] / tt; } else { u1 = 0.0; @@ -179,15 +191,15 @@ void fcn(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int i double z = ZO[i]; /* coordonnees du point i dans le repere camera */ - double rx = (rd[0][0] * x) + (rd[0][1] * y) + (rd[0][2] * z) + xc[0]; - double ry = (rd[1][0] * x) + (rd[1][1] * y) + (rd[1][2] * z) + xc[1]; - double rz = (rd[2][0] * x) + (rd[2][1] * y) + (rd[2][2] * z) + xc[2]; + double rx = (rd[index_0][index_0] * x) + (rd[index_0][index_1] * y) + (rd[index_0][index_2] * z) + xc[index_0]; + double ry = (rd[index_1][index_0] * x) + (rd[index_1][index_1] * y) + (rd[index_1][index_2] * z) + xc[index_1]; + double rz = (rd[index_2][index_0] * x) + (rd[index_2][index_1] * y) + (rd[index_2][index_2] * z) + xc[index_2]; /* derive des fonctions rx, ry et rz par rapport * a tt, u1, u2, u3. */ double drxt = (((si * u1 * u3) + (co * u2)) * z) + (((si * u1 * u2) - (co * u3)) * y) + (((si * u1 * u1) - si) * x); - double drxu1 = (mco * u3 * z) + (mco * u2 * y) + (2 * mco * u1 * x); + double drxu1 = (mco * u3 * z) + (mco * u2 * y) + (2. * mco * u1 * x); double drxu2 = (si * z) + (mco * u1 * y); double drxu3 = (mco * u1 * z) - (si * y); @@ -221,35 +233,35 @@ void fcn(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int i * derivee de la fonction representant le modele de la * camera par rapport aux parametres. */ - *MIJ(jac, 0, i, ldfjac) = 1 / rz; - *MIJ(jac, 1, i, ldfjac) = 0.0; - *MIJ(jac, 2, i, ldfjac) = -rx / (rz * rz); + *MIJ(jac, index_0, i, ldfjac) = 1. / rz; + *MIJ(jac, index_1, i, ldfjac) = 0.0; + *MIJ(jac, index_2, i, ldfjac) = -rx / (rz * rz); if (tt >= MINIMUM) { - *MIJ(jac, 3, i, ldfjac) = (((u1 * dxit) + (((1 - (u1 * u1)) * dxiu1) / tt)) - ((u1 * u2 * dxiu2) / tt)) - ((u1 * u3 * dxiu3) / tt); - *MIJ(jac, 4, i, ldfjac) = (((u2 * dxit) - ((u1 * u2 * dxiu1) / tt)) + (((1 - (u2 * u2)) * dxiu2) / tt)) - ((u2 * u3 * dxiu3) / tt); + *MIJ(jac, index_3, i, ldfjac) = (((u1 * dxit) + (((1. - (u1 * u1)) * dxiu1) / tt)) - ((u1 * u2 * dxiu2) / tt)) - ((u1 * u3 * dxiu3) / tt); + *MIJ(jac, index_4, i, ldfjac) = (((u2 * dxit) - ((u1 * u2 * dxiu1) / tt)) + (((1. - (u2 * u2)) * dxiu2) / tt)) - ((u2 * u3 * dxiu3) / tt); - *MIJ(jac, 5, i, ldfjac) = (((u3 * dxit) - ((u1 * u3 * dxiu1) / tt)) - ((u2 * u3 * dxiu2) / tt)) + (((1 - (u3 * u3)) * dxiu3) / tt); + *MIJ(jac, index_5, i, ldfjac) = (((u3 * dxit) - ((u1 * u3 * dxiu1) / tt)) - ((u2 * u3 * dxiu2) / tt)) + (((1. - (u3 * u3)) * dxiu3) / tt); } else { - *MIJ(jac, 3, i, ldfjac) = 0.0; - *MIJ(jac, 4, i, ldfjac) = 0.0; - *MIJ(jac, 5, i, ldfjac) = 0.0; + *MIJ(jac, index_3, i, ldfjac) = 0.0; + *MIJ(jac, index_4, i, ldfjac) = 0.0; + *MIJ(jac, index_5, i, ldfjac) = 0.0; } - *MIJ(jac, 0, npt + i, ldfjac) = 0.0; - *MIJ(jac, 1, npt + i, ldfjac) = 1 / rz; - *MIJ(jac, 2, npt + i, ldfjac) = -ry / (rz * rz); + *MIJ(jac, index_0, npt + i, ldfjac) = 0.0; + *MIJ(jac, index_1, npt + i, ldfjac) = 1 / rz; + *MIJ(jac, index_2, npt + i, ldfjac) = -ry / (rz * rz); if (tt >= MINIMUM) { - *MIJ(jac, 3, npt + i, ldfjac) = + *MIJ(jac, index_3, npt + i, ldfjac) = (((u1 * dyit) + (((1 - (u1 * u1)) * dyiu1) / tt)) - ((u1 * u2 * dyiu2) / tt)) - ((u1 * u3 * dyiu3) / tt); - *MIJ(jac, 4, npt + i, ldfjac) = - (((u2 * dyit) - ((u1 * u2 * dyiu1) / tt)) + (((1 - (u2 * u2)) * dyiu2) / tt)) - ((u2 * u3 * dyiu3) / tt); - *MIJ(jac, 5, npt + i, ldfjac) = - (((u3 * dyit) - ((u1 * u3 * dyiu1) / tt)) - ((u2 * u3 * dyiu2) / tt)) + (((1 - (u3 * u3)) * dyiu3) / tt); + *MIJ(jac, index_4, npt + i, ldfjac) = + (((u2 * dyit) - ((u1 * u2 * dyiu1) / tt)) + (((1. - (u2 * u2)) * dyiu2) / tt)) - ((u2 * u3 * dyiu3) / tt); + *MIJ(jac, index_5, npt + i, ldfjac) = + (((u3 * dyit) - ((u1 * u3 * dyiu1) / tt)) - ((u2 * u3 * dyiu2) / tt)) + (((1. - (u3 * u3)) * dyiu3) / tt); } else { - *MIJ(jac, 3, npt + i, ldfjac) = 0.0; - *MIJ(jac, 4, npt + i, ldfjac) = 0.0; - *MIJ(jac, 5, npt + i, ldfjac) = 0.0; + *MIJ(jac, index_3, npt + i, ldfjac) = 0.0; + *MIJ(jac, index_4, npt + i, ldfjac) = 0.0; + *MIJ(jac, index_5, npt + i, ldfjac) = 0.0; } } } /* fin else if iflag ==2 */ @@ -257,23 +269,19 @@ void fcn(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int i void vpPose::poseLowe(vpHomogeneousMatrix &cMo) { -#if (DEBUG_LEVEL1) - std::cout << "begin CCalcuvpPose::PoseLowe(...) " << std::endl; -#endif - int n, m; /* nombre d'elements dans la matrice jac */ - int lwa; /* taille du vecteur wa */ - int ldfjac; /* taille maximum d'une ligne de jac */ + /* nombre d'elements dans la matrice jac */ + const int n = NBR_PAR; /* nombres d'inconnues */ + const int m = static_cast(2 * npt); /* nombres d'equations */ + + const int lwa = (2 * NBPTMAX) + 50; /* taille du vecteur wa */ + const int ldfjac = 2 * NBPTMAX; /* taille maximum d'une ligne de jac */ int info, ipvt[NBR_PAR]; int tst_lmder; - double f[2 * NBPTMAX], sol[NBR_PAR]; - double tol, jac[NBR_PAR][2 * NBPTMAX], wa[(2 * NBPTMAX) + 50]; + double f[ldfjac], sol[NBR_PAR]; + double tol, jac[NBR_PAR][ldfjac], wa[lwa]; // --comment: double u of 3 (vecteur de rotation) // --comment: double rd of 3 by 3 (matrice de rotation) - n = NBR_PAR; /* nombres d'inconnues */ - m = static_cast(2 * npt); /* nombres d'equations */ - lwa = (2 * NBPTMAX) + 50; /* taille du vecteur de travail */ - ldfjac = 2 * NBPTMAX; /* nombre d'elements max sur une ligne */ tol = std::numeric_limits::epsilon(); /* critere d'arret */ // --comment: c eq cam @@ -283,9 +291,10 @@ void vpPose::poseLowe(vpHomogeneousMatrix &cMo) vpRotationMatrix cRo; cMo.extract(cRo); vpThetaUVector u(cRo); - for (unsigned int i = 0; i < 3; ++i) { - sol[i] = cMo[i][3]; - sol[i + 3] = u[i]; + const unsigned int val_3 = 3; + for (unsigned int i = 0; i < val_3; ++i) { + sol[i] = cMo[i][val_3]; + sol[i + val_3] = u[i]; } vpPoint P; @@ -307,29 +316,20 @@ void vpPose::poseLowe(vpHomogeneousMatrix &cMo) // --comment: return FATAL ERROR } - for (unsigned int i = 0; i < 3; ++i) { - u[i] = sol[i + 3]; + for (unsigned int i = 0; i < val_3; ++i) { + u[i] = sol[i + val_3]; } - for (unsigned int i = 0; i < 3; ++i) { - cMo[i][3] = sol[i]; - u[i] = sol[i + 3]; + for (unsigned int i = 0; i < val_3; ++i) { + cMo[i][val_3] = sol[i]; + u[i] = sol[i + val_3]; } vpRotationMatrix rd(u); cMo.insert(rd); - // --comment: rot_mat of u comma rd - // --comment: for i eq 0 to 3, for j eq 0 to 3 cMo[i][j] = rd[i][j] - -#if (DEBUG_LEVEL1) - std::cout << "end CCalculPose::PoseLowe(...) " << std::endl; -#endif - // --comment: return OK } +END_VISP_NAMESPACE + #undef MINI #undef MINIMUM - -#undef DEBUG_LEVEL1 - -END_VISP_NAMESPACE diff --git a/modules/vision/src/pose-estimation/vpPoseRGBD.cpp b/modules/vision/src/pose-estimation/vpPoseRGBD.cpp index 93341e04ed..651de31321 100644 --- a/modules/vision/src/pose-estimation/vpPoseRGBD.cpp +++ b/modules/vision/src/pose-estimation/vpPoseRGBD.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -43,14 +43,18 @@ BEGIN_VISP_NAMESPACE void estimatePlaneEquationSVD(const std::vector &point_cloud_face, vpPlane &plane_equation_estimated, vpColVector ¢roid, double &normalized_weights) { - unsigned int max_iter = 10; + const unsigned int max_iter = 10; double prev_error = 1e3; double error = 1e3 - 1; - unsigned int nPoints = static_cast(point_cloud_face.size() / 3); + const unsigned int size3DPt = 3; + unsigned int nPoints = static_cast(point_cloud_face.size() / size3DPt); + const unsigned int idX = 0; + const unsigned int idY = 1; + const unsigned int idZ = 2; vpColVector weights(nPoints, 1.0); vpColVector residues(nPoints); - vpMatrix M(nPoints, 3); + vpMatrix M(nPoints, size3DPt); vpRobust tukey; tukey.setMinMedianAbsoluteDeviation(1e-4); vpColVector normal; @@ -67,9 +71,9 @@ void estimatePlaneEquationSVD(const std::vector &point_cloud_face, vpPla double total_w = 0.0; for (unsigned int i = 0; i < nPoints; ++i) { - centroid_x += weights[i] * point_cloud_face[(3 * i) + 0]; - centroid_y += weights[i] * point_cloud_face[(3 * i) + 1]; - centroid_z += weights[i] * point_cloud_face[(3 * i) + 2]; + centroid_x += weights[i] * point_cloud_face[(size3DPt * i) + idX]; + centroid_y += weights[i] * point_cloud_face[(size3DPt * i) + idY]; + centroid_z += weights[i] * point_cloud_face[(size3DPt * i) + idZ]; total_w += weights[i]; } @@ -79,9 +83,9 @@ void estimatePlaneEquationSVD(const std::vector &point_cloud_face, vpPla // Minimization for (unsigned int i = 0; i < nPoints; ++i) { - M[static_cast(i)][0] = weights[i] * (point_cloud_face[(3 * i) + 0] - centroid_x); - M[static_cast(i)][1] = weights[i] * (point_cloud_face[(3 * i) + 1] - centroid_y); - M[static_cast(i)][2] = weights[i] * (point_cloud_face[(3 * i) + 2] - centroid_z); + M[static_cast(i)][idX] = weights[i] * (point_cloud_face[(size3DPt * i) + idX] - centroid_x); + M[static_cast(i)][idY] = weights[i] * (point_cloud_face[(size3DPt * i) + idY] - centroid_y); + M[static_cast(i)][idZ] = weights[i] * (point_cloud_face[(size3DPt * i) + idZ] - centroid_z); } vpColVector W; @@ -102,15 +106,17 @@ void estimatePlaneEquationSVD(const std::vector &point_cloud_face, vpPla normal = V.getCol(indexSmallestSv); // Compute plane equation - double A = normal[0], B = normal[1], C = normal[2]; + double A = normal[idX]; + double B = normal[idY]; + double C = normal[idZ]; double D = -((A * centroid_x) + (B * centroid_y) + (C * centroid_z)); // Compute error points to estimated plane prev_error = error; error = 0.0; for (unsigned int i = 0; i < nPoints; ++i) { - residues[i] = std::fabs((A * point_cloud_face[3 * i]) + (B * point_cloud_face[(3 * i) + 1]) + - (C * point_cloud_face[(3 * i) + 2]) + D) / + residues[i] = std::fabs((A * point_cloud_face[size3DPt * i]) + (B * point_cloud_face[(size3DPt * i) + idY]) + + (C * point_cloud_face[(size3DPt * i) + idZ]) + D) / sqrt((A * A) + (B * B) + (C * C)); error += weights[i] * residues[i]; } @@ -125,23 +131,23 @@ void estimatePlaneEquationSVD(const std::vector &point_cloud_face, vpPla tukey.MEstimator(vpRobust::TUKEY, residues, weights); // Update final centroid - centroid.resize(3, false); + centroid.resize(size3DPt, false); double total_w = 0.0; for (unsigned int i = 0; i < nPoints; ++i) { - centroid[0] += weights[i] * point_cloud_face[3 * i]; - centroid[1] += weights[i] * point_cloud_face[(3 * i) + 1]; - centroid[2] += weights[i] * point_cloud_face[(3 * i) + 2]; + centroid[idX] += weights[i] * point_cloud_face[size3DPt * i]; + centroid[idY] += weights[i] * point_cloud_face[(size3DPt * i) + idY]; + centroid[idZ] += weights[i] * point_cloud_face[(size3DPt * i) + idZ]; total_w += weights[i]; } - centroid[0] /= total_w; - centroid[1] /= total_w; - centroid[2] /= total_w; + centroid[idX] /= total_w; + centroid[idY] /= total_w; + centroid[idZ] /= total_w; // Compute final plane equation - double A = normal[0], B = normal[1], C = normal[2]; - double D = -((A * centroid[0]) + (B * centroid[1]) + (C * centroid[2])); + double A = normal[0], B = normal[1], C = normal[idZ]; + double D = -((A * centroid[0]) + (B * centroid[1]) + (C * centroid[idZ])); // Update final plane equation plane_equation_estimated.setABCD(A, B, C, D); @@ -248,19 +254,19 @@ bool vpPose::computePlanarObjectPoseFromRGBD(const vpImage &depthMap, const std::vector > &point3d, vpHomogeneousMatrix &cMo, double *confidence_index, bool coplanar_points) { - - if (corners.size() != point3d.size()) { + const size_t nb3dPoints = point3d.size(); + const size_t nbCorners = corners.size(); + if (nbCorners != nb3dPoints) { throw(vpException(vpException::fatalError, "Cannot compute pose from RGBD, 3D (%d) and 2D (%d) data doesn't have the same size", - point3d.size(), corners.size())); + nb3dPoints, nbCorners)); } std::vector pose_points; if (confidence_index != nullptr) { *confidence_index = 0.0; } - size_t point3d_size = point3d.size(); - for (size_t i = 0; i < point3d_size; ++i) { + for (size_t i = 0; i < nb3dPoints; ++i) { std::vector tagPoint3d = point3d[i]; size_t tagpoint3d_size = tagPoint3d.size(); for (size_t j = 0; j < tagpoint3d_size; ++j) { @@ -322,15 +328,17 @@ bool vpPose::computePlanarObjectPoseFromRGBD(const vpImage &depthMap, } size_t nb_points_3d = 0; + const size_t sizePt3D = 3; if (coplanar_points) { - nb_points_3d = tag_points_3d.size() / 3; + nb_points_3d = tag_points_3d.size() / sizePt3D; } else { - nb_points_3d = nb_points_3d_non_planar / 3; + nb_points_3d = nb_points_3d_non_planar / sizePt3D; } - if (nb_points_3d > 4) { + const size_t minNbPts = 4; + if (nb_points_3d > minNbPts) { std::vector p, q; // Plane equation @@ -342,8 +350,7 @@ bool vpPose::computePlanarObjectPoseFromRGBD(const vpImage &depthMap, // If all objects are coplanar, use points insides tag_points_3d to estimate the plane estimatePlaneEquationSVD(tag_points_3d, plane_equation, centroid, normalized_weights); int count = 0; - size_t corners_size = corners.size(); - for (size_t j = 0; j < corners_size; ++j) { + for (size_t j = 0; j < nbCorners; ++j) { std::vector tag_corner = corners[j]; size_t tag_corner_size = tag_corner.size(); for (size_t i = 0; i < tag_corner_size; ++i) { @@ -370,8 +377,9 @@ bool vpPose::computePlanarObjectPoseFromRGBD(const vpImage &depthMap, for (size_t k = 0; k < tag_points_3d_nonplanar_size; ++k) { std::vector rec_points_3d = tag_points_3d_nonplanar[k]; double tag_normalized_weights = 0; - - if (rec_points_3d.size() >= 9) { + const size_t minNbPtsForPlaneSVD = 3; + const size_t minSizeForPlaneSVD = minNbPtsForPlaneSVD * sizePt3D; + if (rec_points_3d.size() >= minSizeForPlaneSVD) { // The array must has at least 3 points for the function estimatePlaneEquationSVD not to crash estimatePlaneEquationSVD(rec_points_3d, plane_equation, centroid, tag_normalized_weights); normalized_weights += tag_normalized_weights; @@ -405,8 +413,7 @@ bool vpPose::computePlanarObjectPoseFromRGBD(const vpImage &depthMap, normalized_weights = normalized_weights / tag_points_3d_nonplanar.size(); } - size_t point3d_size = point3d.size(); - for (size_t i = 0; i < point3d_size; ++i) { + for (size_t i = 0; i < nb3dPoints; ++i) { std::vector tagPoint3d = point3d[i]; // Sometimes an object may do not have enough points registered due to small size. // The issue happens with Orbbec camera while Realsenses was fine. diff --git a/modules/vision/src/pose-estimation/vpPoseRansac.cpp b/modules/vision/src/pose-estimation/vpPoseRansac.cpp index c461804263..0dc41da899 100644 --- a/modules/vision/src/pose-estimation/vpPoseRansac.cpp +++ b/modules/vision/src/pose-estimation/vpPoseRansac.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -53,9 +53,9 @@ #include #endif -BEGIN_VISP_NAMESPACE #define EPS 1e-6 +BEGIN_VISP_NAMESPACE namespace { // For std::map @@ -63,6 +63,9 @@ struct CompareObjectPointDegenerate { bool operator()(const vpPoint &point1, const vpPoint &point2) const { + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; bool rc = false; const double dist1 = (point1.get_oX() * point1.get_oX()) + (point1.get_oY() * point1.get_oY()) + (point1.get_oZ() * point1.get_oZ()); @@ -76,24 +79,24 @@ struct CompareObjectPointDegenerate return false; } - if ((point1.oP[0] - point2.oP[0]) < -EPS) { + if ((point1.oP[index_0] - point2.oP[index_0]) < -EPS) { return true; } - if ((point1.oP[0] - point2.oP[0]) > EPS) { + if ((point1.oP[index_0] - point2.oP[index_0]) > EPS) { return false; } - if ((point1.oP[1] - point2.oP[1]) < -EPS) { + if ((point1.oP[index_1] - point2.oP[index_1]) < -EPS) { return true; } - if ((point1.oP[1] - point2.oP[1]) > EPS) { + if ((point1.oP[index_1] - point2.oP[index_1]) > EPS) { return false; } - if ((point1.oP[2] - point2.oP[2]) < -EPS) { + if ((point1.oP[index_2] - point2.oP[index_2]) < -EPS) { return true; } - if ((point1.oP[2] - point2.oP[2]) > EPS) { + if ((point1.oP[index_2] - point2.oP[index_2]) > EPS) { return false; } @@ -141,9 +144,12 @@ struct FindDegeneratePoint bool operator()(const vpPoint &pt) { - bool result_cond1 = ((std::fabs(m_pt.oP[0] - pt.oP[0]) < EPS) && (std::fabs(m_pt.oP[1] - pt.oP[1]) < EPS) - && (std::fabs(m_pt.oP[2] - pt.oP[2]) < EPS)); - bool result_cond2 = (std::fabs(m_pt.p[0] - pt.p[0]) < EPS) && (std::fabs(m_pt.p[1] - pt.p[1]) < EPS); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + bool result_cond1 = ((std::fabs(m_pt.oP[index_0] - pt.oP[index_0]) < EPS) && (std::fabs(m_pt.oP[index_1] - pt.oP[index_1]) < EPS) + && (std::fabs(m_pt.oP[index_2] - pt.oP[index_2]) < EPS)); + bool result_cond2 = (std::fabs(m_pt.p[index_0] - pt.p[index_0]) < EPS) && (std::fabs(m_pt.p[index_1] - pt.p[index_1]) < EPS); return result_cond1 || result_cond2; } @@ -154,7 +160,7 @@ struct FindDegeneratePoint bool vpPose::vpRansacFunctor::poseRansacImpl() { const unsigned int size = static_cast(m_listOfUniquePoints.size()); - unsigned int nbMinRandom = 4; + const unsigned int nbMinRandom = 4; int nbTrials = 0; vpPoint p; // Point used to project using the estimated pose @@ -215,97 +221,100 @@ bool vpPose::vpRansacFunctor::poseRansacImpl() } } + bool stop_for_loop = false; if (poseMin.npt < nbMinRandom) { ++nbTrials; - continue; + stop_for_loop = true;; } - bool is_pose_valid = false; - double r_min = DBL_MAX; + if (!stop_for_loop) { + bool is_pose_valid = false; + double r_min = DBL_MAX; - try { - is_pose_valid = poseMin.computePose(vpPose::DEMENTHON_LAGRANGE_VIRTUAL_VS, cMo_tmp); - r_min = poseMin.computeResidual(cMo_tmp); - } - catch (...) { - // no need to take action - } + try { + is_pose_valid = poseMin.computePose(vpPose::DEMENTHON_LAGRANGE_VIRTUAL_VS, cMo_tmp); + r_min = poseMin.computeResidual(cMo_tmp); + } + catch (...) { + // no need to take action + } - // If residual returned is not a number (NAN), set valid to false - if (vpMath::isNaN(r_min)) { - is_pose_valid = false; - } + // If residual returned is not a number (NAN), set valid to false + if (vpMath::isNaN(r_min)) { + is_pose_valid = false; + } - // If at pose computation is OK we can continue, otherwise pick another random set - if (is_pose_valid) { - double r = sqrt(r_min) / static_cast(nbMinRandom); // FS should be r = sqrt(r_min / (double)nbMinRandom); - // Filter the pose using some criterion (orientation angles, - // translations, etc.) - bool isPoseValid = true; - if (m_func != nullptr) { - isPoseValid = m_func(cMo_tmp); - if (isPoseValid) { + // If at pose computation is OK we can continue, otherwise pick another random set + if (is_pose_valid) { + double r = sqrt(r_min) / static_cast(nbMinRandom); // FS should be r = sqrt(r_min / (double)nbMinRandom); + // Filter the pose using some criterion (orientation angles, + // translations, etc.) + bool isPoseValid = true; + if (m_func != nullptr) { + isPoseValid = m_func(cMo_tmp); + if (isPoseValid) { + m_cMo = cMo_tmp; + } + } + else { + // No post filtering on pose, so copy cMo_temp to cMo m_cMo = cMo_tmp; } - } - else { - // No post filtering on pose, so copy cMo_temp to cMo - m_cMo = cMo_tmp; - } - if (isPoseValid && (r < m_ransacThreshold)) { - unsigned int nbInliersCur = 0; - unsigned int iter = 0; - std::vector::const_iterator m_listofuniquepoints_end = m_listOfUniquePoints.end(); - for (std::vector::const_iterator it = m_listOfUniquePoints.begin(); it != m_listofuniquepoints_end; - ++it, ++iter) { - p.setWorldCoordinates(it->get_oX(), it->get_oY(), it->get_oZ()); - p.track(m_cMo); - - double error = sqrt(vpMath::sqr(p.get_x() - it->get_x()) + vpMath::sqr(p.get_y() - it->get_y())); - if (error < m_ransacThreshold) { - bool degenerate = false; - if (m_checkDegeneratePoints) { - if (std::find_if(cur_inliers.begin(), cur_inliers.end(), FindDegeneratePoint(*it)) != cur_inliers.end()) { - degenerate = true; + if (isPoseValid && (r < m_ransacThreshold)) { + unsigned int nbInliersCur = 0; + unsigned int iter = 0; + std::vector::const_iterator m_listofuniquepoints_end = m_listOfUniquePoints.end(); + for (std::vector::const_iterator it = m_listOfUniquePoints.begin(); it != m_listofuniquepoints_end; + ++it, ++iter) { + p.setWorldCoordinates(it->get_oX(), it->get_oY(), it->get_oZ()); + p.track(m_cMo); + + double error = sqrt(vpMath::sqr(p.get_x() - it->get_x()) + vpMath::sqr(p.get_y() - it->get_y())); + if (error < m_ransacThreshold) { + bool degenerate = false; + if (m_checkDegeneratePoints) { + if (std::find_if(cur_inliers.begin(), cur_inliers.end(), FindDegeneratePoint(*it)) != cur_inliers.end()) { + degenerate = true; + } } - } - if (!degenerate) { - // the point is considered as inlier if the error is below the - // threshold - ++nbInliersCur; - cur_consensus.push_back(iter); - cur_inliers.push_back(*it); + if (!degenerate) { + // the point is considered as inlier if the error is below the + // threshold + ++nbInliersCur; + cur_consensus.push_back(iter); + cur_inliers.push_back(*it); + } + else { + cur_outliers.push_back(iter); + } } else { cur_outliers.push_back(iter); } } - else { - cur_outliers.push_back(iter); + + if (nbInliersCur > m_nbInliers) { + foundSolution = true; + m_best_consensus = cur_consensus; + m_nbInliers = nbInliersCur; } - } - if (nbInliersCur > m_nbInliers) { - foundSolution = true; - m_best_consensus = cur_consensus; - m_nbInliers = nbInliersCur; - } + ++nbTrials; - ++nbTrials; - - if (nbTrials >= m_ransacMaxTrials) { - foundSolution = true; + if (nbTrials >= m_ransacMaxTrials) { + foundSolution = true; + } + } + else { + ++nbTrials; } } else { ++nbTrials; } } - else { - ++nbTrials; - } } return foundSolution; @@ -328,7 +337,8 @@ bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneo vpHomogeneousMatrix cMo_lagrange, cMo_dementhon; - if (listOfPoints.size() < 4) { + const size_t minNbPoints = 4; + if (listOfPoints.size() < minNbPoints) { throw(vpPoseException(vpPoseException::notInitializedError, "Not enough point to compute the pose")); } @@ -375,7 +385,8 @@ bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneo } } - if (listOfUniquePoints.size() < 4) { + const unsigned int minNbUniquePts = 4; + if (listOfUniquePoints.size() < minNbUniquePts) { throw(vpPoseException(vpPoseException::notInitializedError, "Not enough point to compute the pose")); } @@ -463,7 +474,7 @@ bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneo } if (foundSolution) { - unsigned int nbMinRandom = 4; + const unsigned int nbMinRandom = 4; // --comment: print the nbInliers // Display the random picked points (cur_randoms) @@ -573,10 +584,8 @@ void vpPose::findMatch(std::vector &p2D, std::vector &p3D, } } - if (pose.listP.size() < 4) { - vpERROR_TRACE("Ransac method cannot be used in that case "); - vpERROR_TRACE("(at least 4 points are required)"); - vpERROR_TRACE("Not enough point (%d) to compute the pose ", pose.listP.size()); + const size_t minNbPts = 4; + if (pose.listP.size() < minNbPts) { throw(vpPoseException(vpPoseException::notEnoughPointError, "Not enough point (%d) to compute the pose by ransac", pose.listP.size())); } diff --git a/modules/vision/src/pose-estimation/vpPoseVirtualVisualServoing.cpp b/modules/vision/src/pose-estimation/vpPoseVirtualVisualServoing.cpp index 0046accfd0..9fa6046e5f 100644 --- a/modules/vision/src/pose-estimation/vpPoseVirtualVisualServoing.cpp +++ b/modules/vision/src/pose-estimation/vpPoseVirtualVisualServoing.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -47,18 +47,25 @@ BEGIN_VISP_NAMESPACE void vpPose::poseVirtualVS(vpHomogeneousMatrix &cMo) { try { - double residu_1 = 1e8; double r = 1e8 - 1; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + const unsigned int index_5 = 5; + const unsigned int dim2DPoints = 2; // we stop the minimization when the error is bellow 1e-8 int iter = 0; unsigned int nb = static_cast(listP.size()); - vpMatrix L(2 * nb, 6); - vpColVector err(2 * nb); - vpColVector sd(2 * nb), s(2 * nb); + const unsigned int nbColsL = 6; + vpMatrix L(dim2DPoints * nb, nbColsL); + vpColVector err(dim2DPoints * nb); + vpColVector sd(dim2DPoints * nb), s(dim2DPoints * nb); vpColVector v; vpPoint P; @@ -69,8 +76,8 @@ void vpPose::poseVirtualVS(vpHomogeneousMatrix &cMo) std::list::const_iterator listp_end = listP.end(); for (std::list::const_iterator it = listP.begin(); it != listp_end; ++it) { P = *it; - sd[2 * k] = P.get_x(); - sd[(2 * k) + 1] = P.get_y(); + sd[dim2DPoints * k] = P.get_x(); + sd[(dim2DPoints * k) + 1] = P.get_y(); lP.push_back(P); ++k; } @@ -95,22 +102,22 @@ void vpPose::poseVirtualVS(vpHomogeneousMatrix &cMo) // perspective projection P.track(cMo); - double x = s[2 * k] = P.get_x(); /* point projected from cMo */ - double y = s[(2 * k) + 1] = P.get_y(); + double x = s[dim2DPoints * k] = P.get_x(); /* point projected from cMo */ + double y = s[(dim2DPoints * k) + index_1] = P.get_y(); double Z = P.get_Z(); - L[2 * k][0] = -1 / Z; - L[2 * k][1] = 0; - L[2 * k][2] = x / Z; - L[2 * k][3] = x * y; - L[2 * k][4] = -(1 + (x * x)); - L[2 * k][5] = y; - - L[(2 * k) + 1][0] = 0; - L[(2 * k) + 1][1] = -1 / Z; - L[(2 * k) + 1][2] = y / Z; - L[(2 * k) + 1][3] = 1 + (y * y); - L[(2 * k) + 1][4] = -x * y; - L[(2 * k) + 1][5] = -x; + L[dim2DPoints * k][index_0] = -1 / Z; + L[dim2DPoints * k][index_1] = 0; + L[dim2DPoints * k][index_2] = x / Z; + L[dim2DPoints * k][index_3] = x * y; + L[dim2DPoints * k][index_4] = -(1 + (x * x)); + L[dim2DPoints * k][index_5] = y; + + L[(dim2DPoints * k) + 1][index_0] = 0; + L[(dim2DPoints * k) + 1][index_1] = -1 / Z; + L[(dim2DPoints * k) + 1][index_2] = y / Z; + L[(dim2DPoints * k) + 1][index_3] = 1 + (y * y); + L[(dim2DPoints * k) + 1][index_4] = -x * y; + L[(dim2DPoints * k) + 1][index_5] = -x; k += 1; } @@ -144,129 +151,130 @@ void vpPose::poseVirtualVS(vpHomogeneousMatrix &cMo) covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev, err, L); } } - catch (...) { - vpERROR_TRACE(" "); - throw; + throw(vpException(vpException::fatalError, "poseVirtualVS did not succeed")); } } void vpPose::poseVirtualVSrobust(vpHomogeneousMatrix &cMo) { - try { - - double residu_1 = 1e8; - double r = 1e8 - 1; - - // we stop the minimization when the error is bellow 1e-8 - vpMatrix W; - vpRobust robust; - robust.setMinMedianAbsoluteDeviation(0.00001); - vpColVector w, res; - - unsigned int nb = static_cast(listP.size()); - vpMatrix L(2 * nb, 6); - vpColVector error(2 * nb); - vpColVector sd(2 * nb), s(2 * nb); - vpColVector v; + double residu_1 = 1e8; + double r = 1e8 - 1; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + const unsigned int index_5 = 5; + const unsigned int dim2DPoints = 2; + + // we stop the minimization when the error is bellow 1e-8 + vpMatrix W; + vpRobust robust; + robust.setMinMedianAbsoluteDeviation(0.00001); + vpColVector w, res; + + unsigned int nb = static_cast(listP.size()); + const unsigned int nbColsL = 6; + vpMatrix L(dim2DPoints * nb, nbColsL); + vpColVector error(dim2DPoints * nb); + vpColVector sd(dim2DPoints * nb), s(dim2DPoints * nb); + vpColVector v; + + vpPoint P; + std::list lP; - vpPoint P; - std::list lP; + // create sd + unsigned int k_ = 0; + std::list::const_iterator listp_end = listP.end(); + for (std::list::const_iterator it = listP.begin(); it != listp_end; ++it) { + P = *it; + sd[dim2DPoints * k_] = P.get_x(); + sd[(dim2DPoints * k_) + 1] = P.get_y(); + lP.push_back(P); + ++k_; + } + int iter = 0; + res.resize(s.getRows() / dim2DPoints); + w.resize(s.getRows() / dim2DPoints); + W.resize(s.getRows(), s.getRows()); + w = 1; + + // --comment: while (residu_1 - r) times 1e12 diff 0 + bool iter_gt_vvsitermax = false; + while ((std::fabs((residu_1 - r) * 1e12) > std::numeric_limits::epsilon()) && (iter_gt_vvsitermax == false)) { + residu_1 = r; - // create sd - unsigned int k_ = 0; - std::list::const_iterator listp_end = listP.end(); - for (std::list::const_iterator it = listP.begin(); it != listp_end; ++it) { + // Compute the interaction matrix and the error + k_ = 0; + std::list::const_iterator lp_end = lP.end(); + for (std::list::const_iterator it = lP.begin(); it != lp_end; ++it) { P = *it; - sd[2 * k_] = P.get_x(); - sd[(2 * k_) + 1] = P.get_y(); - lP.push_back(P); + // forward projection of the 3D model for a given pose + // change frame coordinates + // perspective projection + P.track(cMo); + + double x = s[dim2DPoints * k_] = P.get_x(); // point projected from cMo + double y = s[(dim2DPoints * k_) + index_1] = P.get_y(); + double Z = P.get_Z(); + L[dim2DPoints * k_][index_0] = -1 / Z; + L[dim2DPoints * k_][index_1] = 0; + L[dim2DPoints * k_][index_2] = x / Z; + L[dim2DPoints * k_][index_3] = x * y; + L[dim2DPoints * k_][index_4] = -(1 + (x * x)); + L[dim2DPoints * k_][index_5] = y; + + L[(dim2DPoints * k_) + index_1][index_0] = 0; + L[(dim2DPoints * k_) + index_1][index_1] = -1 / Z; + L[(dim2DPoints * k_) + index_1][index_2] = y / Z; + L[(dim2DPoints * k_) + index_1][index_3] = 1 + (y * y); + L[(dim2DPoints * k_) + index_1][index_4] = -x * y; + L[(dim2DPoints * k_) + index_1][index_5] = -x; + ++k_; } - int iter = 0; - res.resize(s.getRows() / 2); - w.resize(s.getRows() / 2); - W.resize(s.getRows(), s.getRows()); - w = 1; + error = s - sd; - // --comment: while (residu_1 - r) times 1e12 diff 0 - bool iter_gt_vvsitermax = false; - while ((std::fabs((residu_1 - r) * 1e12) > std::numeric_limits::epsilon()) && (iter_gt_vvsitermax == false)) { - residu_1 = r; - - // Compute the interaction matrix and the error - k_ = 0; - std::list::const_iterator lp_end = lP.end(); - for (std::list::const_iterator it = lP.begin(); it != lp_end; ++it) { - P = *it; - // forward projection of the 3D model for a given pose - // change frame coordinates - // perspective projection - P.track(cMo); - - double x = s[2 * k_] = P.get_x(); // point projected from cMo - double y = s[(2 * k_) + 1] = P.get_y(); - double Z = P.get_Z(); - L[2 * k_][0] = -1 / Z; - L[2 * k_][1] = 0; - L[2 * k_][2] = x / Z; - L[2 * k_][3] = x * y; - L[2 * k_][4] = -(1 + (x * x)); - L[2 * k_][5] = y; - - L[(2 * k_) + 1][0] = 0; - L[(2 * k_) + 1][1] = -1 / Z; - L[(2 * k_) + 1][2] = y / Z; - L[(2 * k_) + 1][3] = 1 + (y * y); - L[(2 * k_) + 1][4] = -x * y; - L[(2 * k_) + 1][5] = -x; - - ++k_; - } - error = s - sd; - - // compute the residual - r = error.sumSquare(); + // compute the residual + r = error.sumSquare(); - unsigned int v_error_rows = error.getRows(); - for (unsigned int k = 0; k < (v_error_rows / 2); ++k) { - res[k] = vpMath::sqr(error[2 * k]) + vpMath::sqr(error[(2 * k) + 1]); - } - robust.MEstimator(vpRobust::TUKEY, res, w); + unsigned int v_error_rows = error.getRows(); + const unsigned int nbPts = v_error_rows / dim2DPoints; + for (unsigned int k = 0; k < nbPts; ++k) { + res[k] = vpMath::sqr(error[dim2DPoints * k]) + vpMath::sqr(error[(dim2DPoints * k) + 1]); + } + robust.MEstimator(vpRobust::TUKEY, res, w); - // compute the pseudo inverse of the interaction matrix - unsigned int error_rows = error.getRows(); - for (unsigned int k = 0; k < (error_rows / 2); ++k) { - W[2 * k][2 * k] = w[k]; - W[(2 * k) + 1][(2 * k) + 1] = w[k]; - } - // compute the pseudo inverse of the interaction matrix - vpMatrix Lp; - (W * L).pseudoInverse(Lp, 1e-6); + // compute the pseudo inverse of the interaction matrix + unsigned int error_rows = error.getRows(); + unsigned int nbErrors = error_rows / dim2DPoints; + for (unsigned int k = 0; k < nbErrors; ++k) { + W[dim2DPoints * k][dim2DPoints * k] = w[k]; + W[(dim2DPoints * k) + 1][(dim2DPoints * k) + 1] = w[k]; + } + // compute the pseudo inverse of the interaction matrix + vpMatrix Lp; + (W * L).pseudoInverse(Lp, 1e-6); - // compute the VVS control law - v = -m_lambda * Lp * W * error; + // compute the VVS control law + v = -m_lambda * Lp * W * error; - cMo = vpExponentialMap::direct(v).inverse() * cMo; - if (iter > vvsIterMax) { - iter_gt_vvsitermax = true; - // break - } - else { - ++iter; - } + cMo = vpExponentialMap::direct(v).inverse() * cMo; + if (iter > vvsIterMax) { + iter_gt_vvsitermax = true; + // break } - - if (computeCovariance) { - covarianceMatrix = - vpMatrix::computeCovarianceMatrix(L, v, -m_lambda * error, W * W); // Remark: W*W = W*W.t() since the - // matrix is diagonale, but using W*W - // is more efficient. + else { + ++iter; } } - catch (...) { - vpERROR_TRACE(" "); - throw; + + if (computeCovariance) { + covarianceMatrix = + vpMatrix::computeCovarianceMatrix(L, v, -m_lambda * error, W * W); // Remark: W*W = W*W.t() since the + // matrix is diagonale, but using W*W + // is more efficient. } } @@ -277,18 +285,26 @@ std::optional vpPose::poseVirtualVSWithDepth(const std::vec auto residu_1 { 1e8 }, r { 1e8 - 1 }; const auto lambda { 0.9 }, vvsEpsilon { 1e-8 }; const unsigned int vvsIterMax { 200 }; + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + const unsigned int index_5 = 5; const unsigned int nb = static_cast(points.size()); - vpMatrix L(3 * nb, 6); - vpColVector err(3 * nb); - vpColVector sd(3 * nb), s(3 * nb); + const unsigned int sizePoints = 3; + const unsigned int nbColsL = 6; + vpMatrix L(sizePoints * nb, nbColsL); + vpColVector err(sizePoints * nb); + vpColVector sd(sizePoints * nb), s(sizePoints * nb); // create sd auto v_points_size = points.size(); for (auto i = 0u; i < v_points_size; ++i) { - sd[3 * i] = points[i].get_x(); - sd[(3 * i) + 1] = points[i].get_y(); - sd[(3 * i) + 2] = points[i].get_Z(); + sd[sizePoints * i] = points[i].get_x(); + sd[(sizePoints * i) + index_1] = points[i].get_y(); + sd[(sizePoints * i) + index_2] = points[i].get_Z(); } auto cMoPrev = cMo; @@ -306,29 +322,29 @@ std::optional vpPose::poseVirtualVSWithDepth(const std::vec points.at(i).changeFrame(cMo, cP); points.at(i).projection(cP, p); - const auto x = s[3 * i] = p[0]; - const auto y = s[(3 * i) + 1] = p[1]; - const auto Z = s[(3 * i) + 2] = cP[2]; - L[3 * i][0] = -1 / Z; - L[3 * i][1] = 0; - L[3 * i][2] = x / Z; - L[3 * i][3] = x * y; - L[3 * i][4] = -(1 + vpMath::sqr(x)); - L[3 * i][5] = y; - - L[(3 * i) + 1][0] = 0; - L[(3 * i) + 1][1] = -1 / Z; - L[(3 * i) + 1][2] = y / Z; - L[(3 * i) + 1][3] = 1 + vpMath::sqr(y); - L[(3 * i) + 1][4] = -x * y; - L[(3 * i) + 1][5] = -x; - - L[(3 * i) + 2][0] = 0; - L[(3 * i) + 2][1] = 0; - L[(3 * i) + 2][2] = -1; - L[(3 * i) + 2][3] = -y * Z; - L[(3 * i) + 2][4] = x * Z; - L[(3 * i) + 2][5] = -0; + const auto x = s[sizePoints * i] = p[index_0]; + const auto y = s[(sizePoints * i) + index_1] = p[index_1]; + const auto Z = s[(sizePoints * i) + index_2] = cP[index_2]; + L[sizePoints * i][index_0] = -1. / Z; + L[sizePoints * i][index_1] = 0; + L[sizePoints * i][index_2] = x / Z; + L[sizePoints * i][index_3] = x * y; + L[sizePoints * i][index_4] = -(1. + vpMath::sqr(x)); + L[sizePoints * i][index_5] = y; + + L[(sizePoints * i) + index_1][index_0] = 0; + L[(sizePoints * i) + index_1][index_1] = -1. / Z; + L[(sizePoints * i) + index_1][index_2] = y / Z; + L[(sizePoints * i) + index_1][index_3] = 1. + vpMath::sqr(y); + L[(sizePoints * i) + index_1][index_4] = -x * y; + L[(sizePoints * i) + index_1][index_5] = -x; + + L[(sizePoints * i) + index_2][index_0] = 0; + L[(sizePoints * i) + index_2][index_1] = 0; + L[(sizePoints * i) + index_2][index_2] = -1.; + L[(sizePoints * i) + index_2][index_3] = -y * Z; + L[(sizePoints * i) + index_2][index_4] = x * Z; + L[(sizePoints * i) + index_2][index_5] = -0; } err = s - sd; diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureSegment.h b/modules/visual_features/include/visp3/visual_features/vpFeatureSegment.h index c54e442d82..79584140bb 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureSegment.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureSegment.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,10 +36,11 @@ * \brief class that defines the Segment visual feature */ -#ifndef vpFeatureSegment_H -#define vpFeatureSegment_H +#ifndef VP_FEATURE_SEGMENT_H +#define VP_FEATURE_SEGMENT_H #include +#include #include #include #include diff --git a/modules/visual_features/src/feature-builder/vpFeatureBuilderPoint.cpp b/modules/visual_features/src/feature-builder/vpFeatureBuilderPoint.cpp index b6dde258ae..6404a31f60 100644 --- a/modules/visual_features/src/feature-builder/vpFeatureBuilderPoint.cpp +++ b/modules/visual_features/src/feature-builder/vpFeatureBuilderPoint.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,14 +29,14 @@ * * Description: * Conversion between tracker and visual feature point. - * -*****************************************************************************/ + */ /*! \file vpFeatureBuilderPoint.cpp \brief conversion between tracker and visual feature Point */ +#include #include #include #include diff --git a/modules/visual_features/src/feature-builder/vpFeatureBuilderPointPolar.cpp b/modules/visual_features/src/feature-builder/vpFeatureBuilderPointPolar.cpp index 990858e0fe..b469d41bcb 100644 --- a/modules/visual_features/src/feature-builder/vpFeatureBuilderPointPolar.cpp +++ b/modules/visual_features/src/feature-builder/vpFeatureBuilderPointPolar.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,8 +30,7 @@ * Description: * Conversion between tracker and visual feature point with * polar coordinates. - * -*****************************************************************************/ + */ /*! \file vpFeatureBuilderPointPolar.cpp @@ -40,6 +38,7 @@ \brief Conversion between tracker and visual feature point with polar coordinates. */ +#include #include #include #include diff --git a/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp b/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp index 3d281d5239..76d0ddd78e 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp @@ -38,6 +38,7 @@ For more details see \cite Collewet08c. */ +#include #include #include #include diff --git a/modules/visual_features/src/visual-feature/vpFeatureMomentCentered.cpp b/modules/visual_features/src/visual-feature/vpFeatureMomentCentered.cpp index 2c49892f04..fd299dc759 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureMomentCentered.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureMomentCentered.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -35,6 +35,7 @@ #include +#include #include #include #include diff --git a/modules/vs/src/vpServoData.cpp b/modules/vs/src/vpServoData.cpp index 71f359957a..2f79da0dfc 100644 --- a/modules/vs/src/vpServoData.cpp +++ b/modules/vs/src/vpServoData.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -36,11 +36,10 @@ \brief save data during the task execution */ -// Servo -#include - +#include #include #include +#include #include BEGIN_VISP_NAMESPACE From 833f3253a47a2fdd8166196f6e694ac54c40233e Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Tue, 18 Jun 2024 15:37:43 +0200 Subject: [PATCH 02/14] Enable ctest when examples and demos are build --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ec80adb253..58541f03d0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -838,7 +838,7 @@ VP_CHECK_PACKAGE(Log1p) # We use CDash set through CTestConfig.cmake file # Dashboards are sent to https://cdash-ci.inria.fr/index.php?project=ViSP #---------------------------------------------------------------------- -if(BUILD_TESTS) +if(BUILD_TESTS OR BUILD_EXAMPLES OR BUILD_DEMOS) enable_testing() mark_as_advanced(DART_ROOT) mark_as_advanced(BUILD_TESTING) From 9c0db519b555384f15d229585fa66715d126a9c1 Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Tue, 18 Jun 2024 15:57:04 +0200 Subject: [PATCH 03/14] Fix warning delete called on non-final 'vpLuminancePCA' that has virtual functions but non-virtual destructor --- .../visp3/visual_features/vpFeatureLuminanceMapping.h | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h index ae7573bcc5..1b3f88b7ea 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminanceMapping.h @@ -64,6 +64,11 @@ class VISP_EXPORT vpLuminanceMapping */ vpLuminanceMapping(unsigned int mappingSize) : m_mappingSize(mappingSize) { } + /** + * Destructor. + */ + virtual ~vpLuminanceMapping() { } + /** * \brief Map an image \p I to a representation \p s. * This representation s has getProjectionSize() rows. @@ -147,6 +152,11 @@ class VISP_EXPORT vpLuminancePCA : public vpLuminanceMapping public: vpLuminancePCA() : vpLuminanceMapping(0), m_basis(nullptr), m_mean(nullptr), m_Ivec(0), m_Ih(0), m_Iw(0) { } + /** + * Destructor. + */ + virtual ~vpLuminancePCA() { } + /** * \brief Build a new PCA object * From 0b2e783f6a513eb9b46ca0698a5f2ad5ad103f1c Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Tue, 18 Jun 2024 16:02:55 +0200 Subject: [PATCH 04/14] Remove vpDebug.h usage in vpRobust --- modules/core/src/math/robust/vpRobust.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/modules/core/src/math/robust/vpRobust.cpp b/modules/core/src/math/robust/vpRobust.cpp index ab2005ca3f..e5af9ae8b2 100644 --- a/modules/core/src/math/robust/vpRobust.cpp +++ b/modules/core/src/math/robust/vpRobust.cpp @@ -45,7 +45,6 @@ #include #include #include -#include BEGIN_VISP_NAMESPACE /*! @@ -321,8 +320,6 @@ vpRobust::vpRobust(unsigned int n_data) #endif m_size(n_data), m_mad(0) { - vpCDEBUG(2) << "vpRobust constructor reached" << std::endl; - m_normres.resize(n_data); m_sorted_normres.resize(n_data); m_sorted_residues.resize(n_data); @@ -353,8 +350,6 @@ void vpRobust::MEstimator(const vpRobustEstimatorType method, const vpColVector switch (method) { case TUKEY: { psiTukey(m_mad, all_normres, weights); - - vpCDEBUG(2) << "Tukey's function computed" << std::endl; break; } case CAUCHY: { From 2f5f48137fe8d324f8dcdd4903a453ba6cb46a05 Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Tue, 18 Jun 2024 18:28:40 +0200 Subject: [PATCH 05/14] Fix vpHomography usage --- modules/vision/src/pose-estimation/vpPose.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/vision/src/pose-estimation/vpPose.cpp b/modules/vision/src/pose-estimation/vpPose.cpp index 8cb23dec79..6fb7258a7e 100644 --- a/modules/vision/src/pose-estimation/vpPose.cpp +++ b/modules/vision/src/pose-estimation/vpPose.cpp @@ -44,7 +44,7 @@ #include #include #include -#ifdef VISP_POSE_USES_HOMOGRAPHY +#ifdef VISP_HAVE_HOMOGRAPHY #include #endif From f9a790eefdb30c520dde652f22f685da8d8a08ac Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Tue, 18 Jun 2024 18:28:58 +0200 Subject: [PATCH 06/14] Introduce VISP_HAVE_HOMOGRAPHY macro --- CMakeLists.txt | 4 ++++ cmake/templates/VISPConfig.cmake.in | 1 + cmake/templates/vpConfig.h.in | 3 +++ doc/config-doxygen.in | 1 + modules/core/src/math/transformation/vpHomogeneousMatrix.cpp | 2 -- 5 files changed, 9 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 58541f03d0..a7510de777 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1283,6 +1283,10 @@ if(VISP_DATASET_FOUND) set(VISP_HAVE_DATASET_VERSION "(${VISP_DATASET_VERSION_MAJOR}<<16 | ${VISP_DATASET_VERSION_MINOR}<<8 | ${VISP_DATASET_VERSION_PATCH})") # for vpConfig.h endif() +if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/modules/vision/include/visp3/vision/vpHomography.h") + set(VISP_HAVE_HOMOGRAPHY TRUE) +endif() + # libraries for Pioneer mobile robots if(USE_ARIA AND UNIX) if(ARIA_FOUND AND USE_THREADS AND RT_FOUND AND DL_FOUND) diff --git a/cmake/templates/VISPConfig.cmake.in b/cmake/templates/VISPConfig.cmake.in index afceccebe1..87710c01fe 100644 --- a/cmake/templates/VISPConfig.cmake.in +++ b/cmake/templates/VISPConfig.cmake.in @@ -220,6 +220,7 @@ set(VISP_HAVE_FT_IIT_SDK "@VISP_HAVE_FT_IIT_SDK@") set(VISP_HAVE_GDI "@VISP_HAVE_GDI@") set(VISP_HAVE_GTK "@VISP_HAVE_GTK@") set(VISP_HAVE_GSL "@VISP_HAVE_GSL@") +set(VISP_HAVE_HOMOGRAPHY "@VISP_HAVE_HOMOGRAPHY@") set(VISP_HAVE_JACOSDK "@VISP_HAVE_JACOSDK@") set(VISP_HAVE_JPEG "@VISP_HAVE_JPEG@") set(VISP_HAVE_LAPACK "@VISP_HAVE_LAPACK@") diff --git a/cmake/templates/vpConfig.h.in b/cmake/templates/vpConfig.h.in index 13702fed5a..e73c52a4b2 100644 --- a/cmake/templates/vpConfig.h.in +++ b/cmake/templates/vpConfig.h.in @@ -600,6 +600,9 @@ namespace vp = VISP_NAMESPACE_NAME; // Defined if nullptr is available #cmakedefine VISP_HAVE_NULLPTR +// Defined if vpHomography is available +#cmakedefine VISP_HAVE_HOMOGRAPHY + // Emulate nullptr when not available when cxx98 is enabled // Note that on ubuntu 12.04 __cplusplus is equal to 1 that's why in the next line we consider __cplusplus <= 199711L // and not __cplusplus == 199711L diff --git a/doc/config-doxygen.in b/doc/config-doxygen.in index e946a03f85..257d8d2bf1 100644 --- a/doc/config-doxygen.in +++ b/doc/config-doxygen.in @@ -2383,6 +2383,7 @@ PREDEFINED = @DOXYGEN_SHOULD_SKIP_THIS@ \ VISP_HAVE_GTK \ VISP_HAVE_GSL \ VISP_HAVE_FUNC_INET_NTOP \ + VISP_HAVE_HOMOGRAPHY \ VISP_HAVE_JACOSDK \ VISP_HAVE_JPEG \ VISP_HAVE_LAPACK \ diff --git a/modules/core/src/math/transformation/vpHomogeneousMatrix.cpp b/modules/core/src/math/transformation/vpHomogeneousMatrix.cpp index 78c6f94d9f..c172da0265 100644 --- a/modules/core/src/math/transformation/vpHomogeneousMatrix.cpp +++ b/modules/core/src/math/transformation/vpHomogeneousMatrix.cpp @@ -229,11 +229,9 @@ vpHomogeneousMatrix::vpHomogeneousMatrix(const std::initializer_list &li const unsigned int index_0 = 0; const unsigned int index_1 = 1; const unsigned int index_2 = 2; - const unsigned int index_3 = 3; const unsigned int index_4 = 4; const unsigned int index_5 = 5; const unsigned int index_6 = 6; - const unsigned int index_7 = 7; const unsigned int index_8 = 8; const unsigned int index_9 = 9; const unsigned int index_10 = 10; From 14cbdd614a375671313f5414cee79e3ca01c5591 Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Tue, 18 Jun 2024 18:44:43 +0200 Subject: [PATCH 07/14] More sonarqube fixes to shorten functions --- .../core/include/visp3/core/vpImageTools.h | 340 ++++++------- modules/core/include/visp3/core/vpMatrix.h | 8 + modules/core/src/image/vpImageConvert_yuv.cpp | 381 +++----------- modules/core/src/math/matrix/vpMatrix_qr.cpp | 417 +++++++++++----- .../visp3/imgproc/vpCircleHoughTransform.h | 21 +- .../imgproc/src/vpCircleHoughTransform.cpp | 74 +-- .../tracker/me/include/visp3/me/vpMeEllipse.h | 24 + .../me/src/moving-edges/vpMeEllipse.cpp | 402 +++++++-------- modules/vision/include/visp3/vision/vpPose.h | 2 + .../private/vpLevenbergMarquartd.cpp | 466 +++++++++--------- modules/vision/src/pose-estimation/vpPose.cpp | 87 ++-- 11 files changed, 1114 insertions(+), 1108 deletions(-) diff --git a/modules/core/include/visp3/core/vpImageTools.h b/modules/core/include/visp3/core/vpImageTools.h index 303c2e95a6..2c060cf468 100644 --- a/modules/core/include/visp3/core/vpImageTools.h +++ b/modules/core/include/visp3/core/vpImageTools.h @@ -236,6 +236,8 @@ class VISP_EXPORT vpImageTools bool centerCorner, bool fixedPoint); static bool checkFixedPoint(unsigned int x, unsigned int y, const vpMatrix &T, bool affine); + + static void warpLinearFixedPointNotCenter(const vpImage &src, const vpMatrix &T, vpImage &dst, bool affine); }; #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) @@ -1682,197 +1684,203 @@ void vpImageTools::warpLinear(const vpImage &src, const vpMatrix &T, vpIma } } -template <> -inline void vpImageTools::warpLinear(const vpImage &src, const vpMatrix &T, vpImage &dst, bool affine, - bool centerCorner, bool fixedPoint) +inline void vpImageTools::warpLinearFixedPointNotCenter(const vpImage &src, const vpMatrix &T, + vpImage &dst, bool affine) { - const unsigned int index_0 = 0; - const unsigned int index_1 = 1; - const unsigned int index_2 = 2; - if (fixedPoint && (!centerCorner)) { - const int nbits = 16; - const int64_t precision = 1 << nbits; - const float precision_1 = 1 / static_cast(precision); - const int64_t precision2 = 1ULL << (2 * nbits); - const float precision_2 = 1 / static_cast(precision2); - - int64_t a0_i64 = static_cast(T[index_0][index_0] * precision); - int64_t a1_i64 = static_cast(T[index_0][index_1] * precision); - int64_t a2_i64 = static_cast(T[index_0][index_2] * precision); - int64_t a3_i64 = static_cast(T[index_1][index_0] * precision); - int64_t a4_i64 = static_cast(T[index_1][index_1] * precision); - int64_t a5_i64 = static_cast(T[index_1][index_2] * precision); - int64_t a6_i64 = T.getRows() == 3 ? static_cast(T[index_2][index_0] * precision) : 0; - int64_t a7_i64 = T.getRows() == 3 ? static_cast(T[index_2][index_1] * precision) : 0; - int64_t a8_i64 = precision; - - int64_t height_i64 = static_cast(src.getHeight() * precision); - int64_t width_i64 = static_cast(src.getWidth() * precision); - - if (affine) { - unsigned int dst_height = dst.getHeight(); - unsigned int dst_width = dst.getWidth(); - int src_height = static_cast(src.getHeight()); - int src_width = static_cast(src.getWidth()); - for (unsigned int i = 0; i < dst_height; ++i) { - int64_t xi = a2_i64; - int64_t yi = a5_i64; - - for (unsigned int j = 0; j < dst_width; ++j) { - if ((yi >= 0) && (yi < height_i64) && (xi >= 0) && (xi < width_i64)) { - const int64_t xi_lower = xi & (~0xFFFF); - const int64_t yi_lower = yi & (~0xFFFF); + const unsigned int index_0 = 0, index_1 = 1, index_2 = 2; + const int nbits = 16; + const int64_t precision = 1 << nbits; + const float precision_1 = 1.f / static_cast(precision); + const int64_t precision2 = 1ULL << (2 * nbits); + const float precision_2 = 1.f / static_cast(precision2); + + int64_t a0_i64 = static_cast(T[index_0][index_0] * precision); + int64_t a1_i64 = static_cast(T[index_0][index_1] * precision); + int64_t a2_i64 = static_cast(T[index_0][index_2] * precision); + int64_t a3_i64 = static_cast(T[index_1][index_0] * precision); + int64_t a4_i64 = static_cast(T[index_1][index_1] * precision); + int64_t a5_i64 = static_cast(T[index_1][index_2] * precision); + int64_t a6_i64 = T.getRows() == 3 ? static_cast(T[index_2][index_0] * precision) : 0; + int64_t a7_i64 = T.getRows() == 3 ? static_cast(T[index_2][index_1] * precision) : 0; + int64_t a8_i64 = precision; + + int64_t height_i64 = static_cast(src.getHeight() * precision); + int64_t width_i64 = static_cast(src.getWidth() * precision); - const int64_t t = yi - yi_lower; - const int64_t t_1 = precision - t; - const int64_t s = xi - xi_lower; - const int64_t s_1 = precision - s; + if (affine) { + unsigned int dst_height = dst.getHeight(); + unsigned int dst_width = dst.getWidth(); + int src_height = static_cast(src.getHeight()); + int src_width = static_cast(src.getWidth()); + for (unsigned int i = 0; i < dst_height; ++i) { + int64_t xi = a2_i64; + int64_t yi = a5_i64; - const int x_ = static_cast(xi >> nbits); - const int y_ = static_cast(yi >> nbits); + for (unsigned int j = 0; j < dst_width; ++j) { + if ((yi >= 0) && (yi < height_i64) && (xi >= 0) && (xi < width_i64)) { + const int64_t xi_lower = xi & (~0xFFFF); + const int64_t yi_lower = yi & (~0xFFFF); + + const int64_t t = yi - yi_lower; + const int64_t t_1 = precision - t; + const int64_t s = xi - xi_lower; + const int64_t s_1 = precision - s; + + const int x_ = static_cast(xi >> nbits); + const int y_ = static_cast(yi >> nbits); + + if ((y_ < (src_height - 1)) && (x_ < (src_width - 1))) { + const vpRGBa val00 = src[y_][x_]; + const vpRGBa val01 = src[y_][x_ + 1]; + const vpRGBa val10 = src[y_ + 1][x_]; + const vpRGBa val11 = src[y_ + 1][x_ + 1]; + const int64_t interpR_i64 = + static_cast((s_1 * t_1 * val00.R) + (s * t_1 * val01.R) + (s_1 * t * val10.R) + (s * t * val11.R)); + const float interpR = (interpR_i64 >> (nbits * 2)) + ((interpR_i64 & 0xFFFFFFFF) * precision_2); + + const int64_t interpG_i64 = + static_cast((s_1 * t_1 * val00.G) + (s * t_1 * val01.G) + (s_1 * t * val10.G) + (s * t * val11.G)); + const float interpG = (interpG_i64 >> (nbits * 2)) + ((interpG_i64 & 0xFFFFFFFF) * precision_2); + + const int64_t interpB_i64 = + static_cast((s_1 * t_1 * val00.B) + (s * t_1 * val01.B) + (s_1 * t * val10.B) + (s * t * val11.B)); + const float interpB = (interpB_i64 >> (nbits * 2)) + ((interpB_i64 & 0xFFFFFFFF) * precision_2); - if ((y_ < (src_height - 1)) && (x_ < (src_width - 1))) { - const vpRGBa val00 = src[y_][x_]; - const vpRGBa val01 = src[y_][x_ + 1]; - const vpRGBa val10 = src[y_ + 1][x_]; - const vpRGBa val11 = src[y_ + 1][x_ + 1]; - const int64_t interpR_i64 = - static_cast((s_1 * t_1 * val00.R) + (s * t_1 * val01.R) + (s_1 * t * val10.R) + (s * t * val11.R)); - const float interpR = (interpR_i64 >> (nbits * 2)) + ((interpR_i64 & 0xFFFFFFFF) * precision_2); - - const int64_t interpG_i64 = - static_cast((s_1 * t_1 * val00.G) + (s * t_1 * val01.G) + (s_1 * t * val10.G) + (s * t * val11.G)); - const float interpG = (interpG_i64 >> (nbits * 2)) + ((interpG_i64 & 0xFFFFFFFF) * precision_2); - - const int64_t interpB_i64 = - static_cast((s_1 * t_1 * val00.B) + (s * t_1 * val01.B) + (s_1 * t * val10.B) + (s * t * val11.B)); - const float interpB = (interpB_i64 >> (nbits * 2)) + ((interpB_i64 & 0xFFFFFFFF) * precision_2); - - dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), - vpMath::saturate(interpB), 255); - } - 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 float interpR = (interpR_i64 >> nbits) + ((interpR_i64 & 0xFFFF) * precision_1); + dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), + vpMath::saturate(interpB), 255); + } + 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 float interpR = (interpR_i64 >> nbits) + ((interpR_i64 & 0xFFFF) * precision_1); - const int64_t interpG_i64 = static_cast((t_1 * val00.G) + (t * val10.G)); - const float interpG = (interpG_i64 >> nbits) + ((interpG_i64 & 0xFFFF) * precision_1); + const int64_t interpG_i64 = static_cast((t_1 * val00.G) + (t * val10.G)); + const float interpG = (interpG_i64 >> nbits) + ((interpG_i64 & 0xFFFF) * precision_1); - const int64_t interpB_i64 = static_cast((t_1 * val00.B) + (t * val10.B)); - const float interpB = (interpB_i64 >> nbits) + ((interpB_i64 & 0xFFFF) * precision_1); + const int64_t interpB_i64 = static_cast((t_1 * val00.B) + (t * val10.B)); + 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); - } - else if (x_ < (src_width - 1)) { - const vpRGBa val00 = src[y_][x_]; - const vpRGBa val01 = src[y_][x_ + 1]; - const int64_t interpR_i64 = static_cast((s_1 * val00.R) + (s * val01.R)); - const float interpR = (interpR_i64 >> nbits) + ((interpR_i64 & 0xFFFF) * precision_1); + dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), + vpMath::saturate(interpB), 255); + } + else if (x_ < (src_width - 1)) { + const vpRGBa val00 = src[y_][x_]; + const vpRGBa val01 = src[y_][x_ + 1]; + const int64_t interpR_i64 = static_cast((s_1 * val00.R) + (s * val01.R)); + const float interpR = (interpR_i64 >> nbits) + ((interpR_i64 & 0xFFFF) * precision_1); - const int64_t interpG_i64 = static_cast((s_1 * val00.G) + (s * val01.G)); - const float interpG = (interpG_i64 >> nbits) + ((interpG_i64 & 0xFFFF) * precision_1); + const int64_t interpG_i64 = static_cast((s_1 * val00.G) + (s * val01.G)); + const float interpG = (interpG_i64 >> nbits) + ((interpG_i64 & 0xFFFF) * precision_1); - const int64_t interpB_i64 = static_cast((s_1 * val00.B) + (s * val01.B)); - const float interpB = (interpB_i64 >> nbits) + ((interpB_i64 & 0xFFFF) * precision_1); + const int64_t interpB_i64 = static_cast((s_1 * val00.B) + (s * val01.B)); + 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); - } - else { - dst[i][j] = src[y_][x_]; - } + dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), + vpMath::saturate(interpB), 255); + } + else { + dst[i][j] = src[y_][x_]; } - - xi += a0_i64; - yi += a3_i64; } - a2_i64 += a1_i64; - a5_i64 += a4_i64; + xi += a0_i64; + yi += a3_i64; } - } - else { - unsigned int dst_height = dst.getHeight(); - unsigned int dst_width = dst.getWidth(); - int src_height = static_cast(src.getHeight()); - int src_width = static_cast(src.getWidth()); - for (unsigned int i = 0; i < dst_height; ++i) { - int64_t xi = a2_i64; - int64_t yi = a5_i64; - int64_t wi = a8_i64; - for (unsigned int j = 0; j < dst_width; ++j) { - if ((yi >= 0) && (yi <= ((src_height - 1) * wi)) && (xi >= 0) && - (xi <= ((src_width - 1) * wi))) { - const float wi_ = (wi >> nbits) + ((wi & 0xFFFF) * precision_1); - const float xi_ = ((xi >> nbits) + ((xi & 0xFFFF) * precision_1)) / wi_; - const float yi_ = ((yi >> nbits) + ((yi & 0xFFFF) * precision_1)) / wi_; + a2_i64 += a1_i64; + a5_i64 += a4_i64; + } + } + else { + unsigned int dst_height = dst.getHeight(); + unsigned int dst_width = dst.getWidth(); + int src_height = static_cast(src.getHeight()); + int src_width = static_cast(src.getWidth()); + for (unsigned int i = 0; i < dst_height; ++i) { + int64_t xi = a2_i64; + int64_t yi = a5_i64; + int64_t wi = a8_i64; - const int x_ = static_cast(xi_); - const int y_ = static_cast(yi_); + for (unsigned int j = 0; j < dst_width; ++j) { + if ((yi >= 0) && (yi <= ((src_height - 1) * wi)) && (xi >= 0) && + (xi <= ((src_width - 1) * wi))) { + const float wi_ = (wi >> nbits) + ((wi & 0xFFFF) * precision_1); + const float xi_ = ((xi >> nbits) + ((xi & 0xFFFF) * precision_1)) / wi_; + const float yi_ = ((yi >> nbits) + ((yi & 0xFFFF) * precision_1)) / wi_; + + const int x_ = static_cast(xi_); + const int y_ = static_cast(yi_); + + const float t = yi_ - y_; + const float s = xi_ - x_; + + if ((y_ < (src_height - 1)) && (x_ < (src_width - 1))) { + const vpRGBa val00 = src[y_][x_]; + const vpRGBa val01 = src[y_][x_ + 1]; + const vpRGBa val10 = src[y_ + 1][x_]; + const vpRGBa val11 = src[y_ + 1][x_ + 1]; + const float colR0 = lerp(val00.R, val01.R, s); + const float colR1 = lerp(val10.R, val11.R, s); + const float interpR = lerp(colR0, colR1, t); + + const float colG0 = lerp(val00.G, val01.G, s); + const float colG1 = lerp(val10.G, val11.G, s); + const float interpG = lerp(colG0, colG1, t); + + const float colB0 = lerp(val00.B, val01.B, s); + const float colB1 = lerp(val10.B, val11.B, s); + const float interpB = lerp(colB0, colB1, t); - const float t = yi_ - y_; - const float s = xi_ - x_; + dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), + vpMath::saturate(interpB), 255); + } + else if (y_ < (src_height - 1)) { + const vpRGBa val00 = src[y_][x_]; + const vpRGBa val10 = src[y_ + 1][x_]; + const float interpR = lerp(val00.R, val10.R, t); + const float interpG = lerp(val00.G, val10.G, t); + const float interpB = lerp(val00.B, val10.B, t); - if ((y_ < (src_height - 1)) && (x_ < (src_width - 1))) { - const vpRGBa val00 = src[y_][x_]; - const vpRGBa val01 = src[y_][x_ + 1]; - const vpRGBa val10 = src[y_ + 1][x_]; - const vpRGBa val11 = src[y_ + 1][x_ + 1]; - const float colR0 = lerp(val00.R, val01.R, s); - const float colR1 = lerp(val10.R, val11.R, s); - const float interpR = lerp(colR0, colR1, t); - - const float colG0 = lerp(val00.G, val01.G, s); - const float colG1 = lerp(val10.G, val11.G, s); - const float interpG = lerp(colG0, colG1, t); - - const float colB0 = lerp(val00.B, val01.B, s); - const float colB1 = lerp(val10.B, val11.B, s); - const float interpB = lerp(colB0, colB1, t); - - dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), - vpMath::saturate(interpB), 255); - } - else if (y_ < (src_height - 1)) { - const vpRGBa val00 = src[y_][x_]; - const vpRGBa val10 = src[y_ + 1][x_]; - const float interpR = lerp(val00.R, val10.R, t); - const float interpG = lerp(val00.G, val10.G, t); - const float interpB = lerp(val00.B, val10.B, t); - - dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), - vpMath::saturate(interpB), 255); - } - else if (x_ < (src_width - 1)) { - const vpRGBa val00 = src[y_][x_]; - const vpRGBa val01 = src[y_][x_ + 1]; - const float interpR = lerp(val00.R, val01.R, s); - const float interpG = lerp(val00.G, val01.G, s); - const float interpB = lerp(val00.B, val01.B, s); - - dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), - vpMath::saturate(interpB), 255); - } - else { - dst[i][j] = src[y_][x_]; - } + dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), + vpMath::saturate(interpB), 255); } + else if (x_ < (src_width - 1)) { + const vpRGBa val00 = src[y_][x_]; + const vpRGBa val01 = src[y_][x_ + 1]; + const float interpR = lerp(val00.R, val01.R, s); + const float interpG = lerp(val00.G, val01.G, s); + const float interpB = lerp(val00.B, val01.B, s); - xi += a0_i64; - yi += a3_i64; - wi += a6_i64; + dst[i][j] = vpRGBa(vpMath::saturate(interpR), vpMath::saturate(interpG), + vpMath::saturate(interpB), 255); + } + else { + dst[i][j] = src[y_][x_]; + } } - a2_i64 += a1_i64; - a5_i64 += a4_i64; - a8_i64 += a7_i64; + xi += a0_i64; + yi += a3_i64; + wi += a6_i64; } + + a2_i64 += a1_i64; + a5_i64 += a4_i64; + a8_i64 += a7_i64; } } + +} + +template <> +inline void vpImageTools::warpLinear(const vpImage &src, const vpMatrix &T, vpImage &dst, bool affine, + bool centerCorner, bool fixedPoint) +{ + const unsigned int index_0 = 0, index_1 = 1, index_2 = 2; + if (fixedPoint && (!centerCorner)) { + warpLinearFixedPointNotCenter(src, T, dst, affine); + } else { double a0 = T[index_0][index_0]; double a1 = T[index_0][index_1]; diff --git a/modules/core/include/visp3/core/vpMatrix.h b/modules/core/include/visp3/core/vpMatrix.h index 55a7b41d1a..9580918d18 100644 --- a/modules/core/include/visp3/core/vpMatrix.h +++ b/modules/core/include/visp3/core/vpMatrix.h @@ -1189,6 +1189,14 @@ class VISP_EXPORT vpMatrix : public vpArray2D double *x_data, int incx_, double beta, double *y_data, int incy_); static void blas_dsyev(char jobz, char uplo, unsigned int n_, double *a_data, unsigned int lda_, double *w_data, double *work_data, int lwork_, int &info_); + + unsigned int qrPivotLapack(vpMatrix &Q, vpMatrix &R, vpMatrix &P, bool full, bool squareR, + double tol) const; + +#ifdef VISP_HAVE_GSL + unsigned int qrPivotLapackGSL(vpMatrix &Q, vpMatrix &R, vpMatrix &P, bool full, bool squareR, + double tol) const; +#endif #endif static void computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls, diff --git a/modules/core/src/image/vpImageConvert_yuv.cpp b/modules/core/src/image/vpImageConvert_yuv.cpp index 1f714a5f32..5206a557dc 100644 --- a/modules/core/src/image/vpImageConvert_yuv.cpp +++ b/modules/core/src/image/vpImageConvert_yuv.cpp @@ -1108,6 +1108,29 @@ void vpImageConvert::YV12ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned } } +namespace +{ +/** +* +* \brief Subroutine for YVU9 to RGBa conversion. +* +* \param rgb Pointer to the 32-bits RGBA bitmap that should be allocated with a size of width * height * 4. +* \param R Value for the red channel. +* \param G Value for the green channel. +* \param B Value for the blue channel. +* \param a Value for the alpha channel. +*/ +void YVU9ToRGBasubroutine(unsigned char *rgba, int R, int G, int B, int a) +{ + vpSAT(R); + vpSAT(G); + vpSAT(B); + *rgba++ = static_cast(R); + *rgba++ = static_cast(G); + *rgba++ = static_cast(B); + *rgba++ = a; +} +} /*! Convert YVU 9 [Y(NxM), V(N/4xM/4), U(N/4xM/4)] image into a RGBa image. @@ -1182,243 +1205,99 @@ void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne // G = Y - 0.344 U - 0.714 V // B = Y + 1.772 U R = Y0 + V2; - vpSAT(R); - G = Y0 + UV; - vpSAT(G); - B = Y0 + U5; - vpSAT(B); - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba++ = vpRGBa::alpha_default; + YVU9ToRGBasubroutine(rgba, R, G, B, vpRGBa::alpha_default); //--- R = Y1 + V2; - vpSAT(R); - G = Y1 + UV; - vpSAT(G); - B = Y1 + U5; - vpSAT(B); - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba++ = vpRGBa::alpha_default; + YVU9ToRGBasubroutine(rgba, R, G, B, vpRGBa::alpha_default); //--- R = Y2 + V2; - vpSAT(R); - G = Y2 + UV; - vpSAT(G); - B = Y2 + U5; - vpSAT(B); - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba++ = vpRGBa::alpha_default; + YVU9ToRGBasubroutine(rgba, R, G, B, vpRGBa::alpha_default); //--- R = Y3 + V2; - vpSAT(R); - G = Y3 + UV; - vpSAT(G); - B = Y3 + U5; - vpSAT(B); - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba = vpRGBa::alpha_default; + YVU9ToRGBasubroutine(rgba, R, G, B, vpRGBa::alpha_default); rgba = rgba + ((val_4 * width) - val_15); R = Y4 + V2; - vpSAT(R); - G = Y4 + UV; - vpSAT(G); - B = Y4 + U5; - vpSAT(B); - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba++ = vpRGBa::alpha_default; + YVU9ToRGBasubroutine(rgba, R, G, B, vpRGBa::alpha_default); //--- R = Y5 + V2; - vpSAT(R); - G = Y5 + UV; - vpSAT(G); - B = Y5 + U5; - vpSAT(B); - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba++ = vpRGBa::alpha_default; + YVU9ToRGBasubroutine(rgba, R, G, B, vpRGBa::alpha_default); //--- R = Y6 + V2; - vpSAT(R); - G = Y6 + UV; - vpSAT(G); - B = Y6 + U5; - vpSAT(B); - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba++ = vpRGBa::alpha_default; + YVU9ToRGBasubroutine(rgba, R, G, B, vpRGBa::alpha_default); //--- R = Y7 + V2; - vpSAT(R); - G = Y7 + UV; - vpSAT(G); - B = Y7 + U5; - vpSAT(B); - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba = vpRGBa::alpha_default; + YVU9ToRGBasubroutine(rgba, R, G, B, vpRGBa::alpha_default); rgba = rgba + ((val_4 * width) - val_15); R = Y8 + V2; - vpSAT(R); - G = Y8 + UV; - vpSAT(G); - B = Y8 + U5; - vpSAT(B); - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba++ = vpRGBa::alpha_default; + YVU9ToRGBasubroutine(rgba, R, G, B, vpRGBa::alpha_default); //--- R = Y9 + V2; - vpSAT(R); - G = Y9 + UV; - vpSAT(G); - B = Y9 + U5; - vpSAT(B); - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba++ = vpRGBa::alpha_default; + YVU9ToRGBasubroutine(rgba, R, G, B, vpRGBa::alpha_default); //--- R = Y10 + V2; - vpSAT(R); - G = Y10 + UV; - vpSAT(G); - B = Y10 + U5; - vpSAT(B); - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba++ = vpRGBa::alpha_default; + YVU9ToRGBasubroutine(rgba, R, G, B, vpRGBa::alpha_default); //--- R = Y11 + V2; - vpSAT(R); - G = Y11 + UV; - vpSAT(G); - B = Y11 + U5; - vpSAT(B); - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba = vpRGBa::alpha_default; + YVU9ToRGBasubroutine(rgba, R, G, B, vpRGBa::alpha_default); rgba = rgba + ((val_4 * width) - val_15); R = Y12 + V2; - vpSAT(R); - G = Y12 + UV; - vpSAT(G); - B = Y12 + U5; - vpSAT(B); - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba++ = vpRGBa::alpha_default; + YVU9ToRGBasubroutine(rgba, R, G, B, vpRGBa::alpha_default); //--- R = Y13 + V2; - vpSAT(R); - G = Y13 + UV; - vpSAT(G); - B = Y13 + U5; - vpSAT(B); - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba++ = vpRGBa::alpha_default; + YVU9ToRGBasubroutine(rgba, R, G, B, vpRGBa::alpha_default); //--- R = Y14 + V2; - vpSAT(R); - G = Y14 + UV; - vpSAT(G); - B = Y14 + U5; - vpSAT(B); - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba++ = vpRGBa::alpha_default; + YVU9ToRGBasubroutine(rgba, R, G, B, vpRGBa::alpha_default); //--- R = Y15 + V2; - vpSAT(R); - G = Y15 + UV; - vpSAT(G); - B = Y15 + U5; - vpSAT(B); - - *rgba++ = static_cast(R); - *rgba++ = static_cast(G); - *rgba++ = static_cast(B); - *rgba = vpRGBa::alpha_default; + YVU9ToRGBasubroutine(rgba, R, G, B, vpRGBa::alpha_default); rgba = (rgba - (val_12 * width)) + 1; } yuv += val_3 * width; @@ -1426,6 +1305,28 @@ void vpImageConvert::YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigne } } +namespace +{ +/** +* +* \brief Subroutine for YVU9 to RGB conversion. +* +* \param rgb Pointer to the 24-bits RGB bitmap that should be allocated with a size of width * height * 3. +* \param R Value for the red channel. +* \param G Value for the green channel. +* \param B Value for the blue channel. +*/ +void YVU9ToRGBsubroutine(unsigned char *rgb, int R, int G, int B) +{ + vpSAT(R); + vpSAT(G); + vpSAT(B); + *rgb++ = static_cast(R); + *rgb++ = static_cast(G); + *rgb++ = static_cast(B); +} +} + /*! Convert YV 1:2 [Y(NxM), V(N/4xM/4), U(N/4xM/4)] image into RGB image. @@ -1496,227 +1397,99 @@ void vpImageConvert::YVU9ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned // G = Y - 0.344 U - 0.714 V // B = Y + 1.772 U R = Y0 + V2; - vpSAT(R); - G = Y0 + UV; - vpSAT(G); - B = Y0 + U5; - vpSAT(B); - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); + YVU9ToRGBsubroutine(rgb, R, G, B); //--- R = Y1 + V2; - vpSAT(R); - G = Y1 + UV; - vpSAT(G); - B = Y1 + U5; - vpSAT(B); - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); + YVU9ToRGBsubroutine(rgb, R, G, B); //--- R = Y2 + V2; - vpSAT(R); - G = Y2 + UV; - vpSAT(G); - B = Y2 + U5; - vpSAT(B); - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); + YVU9ToRGBsubroutine(rgb, R, G, B); //--- R = Y3 + V2; - vpSAT(R); - G = Y3 + UV; - vpSAT(G); - B = Y3 + U5; - vpSAT(B); - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb = static_cast(B); + YVU9ToRGBsubroutine(rgb, R, G, B); rgb = rgb + ((val_3 * width) - 11); R = Y4 + V2; - vpSAT(R); - G = Y4 + UV; - vpSAT(G); - B = Y4 + U5; - vpSAT(B); - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); + YVU9ToRGBsubroutine(rgb, R, G, B); //--- R = Y5 + V2; - vpSAT(R); - G = Y5 + UV; - vpSAT(G); - B = Y5 + U5; - vpSAT(B); - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); + YVU9ToRGBsubroutine(rgb, R, G, B); //--- R = Y6 + V2; - vpSAT(R); - G = Y6 + UV; - vpSAT(G); - B = Y6 + U5; - vpSAT(B); - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); + YVU9ToRGBsubroutine(rgb, R, G, B); //--- R = Y7 + V2; - vpSAT(R); - G = Y7 + UV; - vpSAT(G); - B = Y7 + U5; - vpSAT(B); - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb = static_cast(B); + YVU9ToRGBsubroutine(rgb, R, G, B); rgb = rgb + ((val_3 * width) - 11); R = Y8 + V2; - vpSAT(R); - G = Y8 + UV; - vpSAT(G); - B = Y8 + U5; - vpSAT(B); - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); + YVU9ToRGBsubroutine(rgb, R, G, B); //--- R = Y9 + V2; - vpSAT(R); - G = Y9 + UV; - vpSAT(G); - B = Y9 + U5; - vpSAT(B); - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); + YVU9ToRGBsubroutine(rgb, R, G, B); //--- R = Y10 + V2; - vpSAT(R); - G = Y10 + UV; - vpSAT(G); - B = Y10 + U5; - vpSAT(B); - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); + YVU9ToRGBsubroutine(rgb, R, G, B); //--- R = Y11 + V2; - vpSAT(R); - G = Y11 + UV; - vpSAT(G); - B = Y11 + U5; - vpSAT(B); - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb = static_cast(B); + YVU9ToRGBsubroutine(rgb, R, G, B); rgb = (rgb + (val_3 * width)) - 11; R = Y12 + V2; - vpSAT(R); - G = Y12 + UV; - vpSAT(G); - B = Y12 + U5; - vpSAT(B); - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); + YVU9ToRGBsubroutine(rgb, R, G, B); //--- R = Y13 + V2; - vpSAT(R); - G = Y13 + UV; - vpSAT(G); - B = Y13 + U5; - vpSAT(B); - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); + YVU9ToRGBsubroutine(rgb, R, G, B); //--- R = Y14 + V2; - vpSAT(R); - G = Y14 + UV; - vpSAT(G); - B = Y14 + U5; - vpSAT(B); - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); + YVU9ToRGBsubroutine(rgb, R, G, B); //--- R = Y15 + V2; - vpSAT(R); - G = Y15 + UV; - vpSAT(G); - B = Y15 + U5; - vpSAT(B); - - *rgb++ = static_cast(R); - *rgb++ = static_cast(G); - *rgb++ = static_cast(B); + YVU9ToRGBsubroutine(rgb, R, G, B); rgb = (rgb - (val_9 * width)) + 1; } yuv += val_3 * width; diff --git a/modules/core/src/math/matrix/vpMatrix_qr.cpp b/modules/core/src/math/matrix/vpMatrix_qr.cpp index ae28d008d2..3cfad7049a 100644 --- a/modules/core/src/math/matrix/vpMatrix_qr.cpp +++ b/modules/core/src/math/matrix/vpMatrix_qr.cpp @@ -654,10 +654,229 @@ unsigned int vpMatrix::qr(vpMatrix &Q, vpMatrix &R, bool full, bool squareR, dou #endif } +#if defined(VISP_HAVE_LAPACK) +#if !defined(VISP_HAVE_GSL) /*! - Compute the QR pivot decomposition of a (m x n) matrix of rank r. - Only available if Lapack 3rd party is installed. If Lapack is not installed - we use a Lapack built-in version. + Compute the QR pivot decomposition of a (m x n) matrix of rank r when + Lapack 3rd party is available. + + \param Q : orthogonal matrix (will be modified). + \param R : upper-triangular matrix (will be modified). + \param P : the (n x n) permutation matrix. + \param full : whether or not we want full decomposition. + \param squareR : will return only the (r x r) part of R and the (r x n) part of P. + \param tol : tolerance to test the rank of R. + + \return The rank r of the matrix. + + If full is false (default) then Q is (m x min(n,m)) and R is (min(n,m) x n). + We then have this.P = Q.R. + + If full is true and m > n then Q is (m x m) and R is (n x n). + In this case this.P = Q (R, 0)^T + + If squareR is true then R is (r x r) invertible. + + Here an example: + \code + #include + + double residual(vpMatrix M1, vpMatrix M2) + { + return (M1 - M2).frobeniusNorm(); + } + + int main() + { + vpMatrix A(4,3); + + A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/2.; + A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; + A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/4.; + A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/5.; + // A is (4x3) but rank 2 + + // Economic QR (Q 4x3, R 3x3) + vpMatrix Q, R, P; + int r = A.qrPivot(Q, R, P); + std::cout << "A rank: " << r << std::endl; + std::cout << "Residual: " << residual(A*P, Q*R) << std::endl; + + // Full QR (Q 4x4, R 3x3) + r = A.qrPivot(Q, R, P, true); + std::cout << "QRPivot Residual: " << + residual(A*P, Q.extract(0, 0, 4, 3)*R) << std::endl; + + // Using permutation matrix: keep only non-null part of R + Q.resize(4, r, false); // Q is 4 x 2 + R = R.extract(0, 0, r, 3)*P.t(); // R is 2 x 3 + std::cout << "Full QRPivot Residual: " << + residual(A, Q*R) << std::endl; + } + \endcode + + \sa qrPivot() +*/ +unsigned int vpMatrix::qrPivotLapack(vpMatrix &Q, vpMatrix &R, vpMatrix &P, bool full, bool squareR, double tol) const +{ + integer m = static_cast(rowNum); // also rows of Q + integer n = static_cast(colNum); // also columns of R + integer r = std::min(n, m); // a priori non-null rows of R = rank of R + integer q = r; // columns of Q and rows of R + integer na = n; // columns of A + + // cannot be full decomposition if m < n + // cannot be full decomposition if m < n + if (full && m > n) { + q = m; // Q is square + na = m; // A is square + } + + // prepare Q and deal with r = 0 + Q.resize(static_cast(m), static_cast(q)); + if (r == 0) { + if (squareR) { + R.resize(0, 0); + P.resize(0, static_cast(n)); + } + else { + R.resize(static_cast(r), static_cast(n)); + P.resize(static_cast(n), static_cast(n)); + } + return 0; + } + + integer dimWork = -1; + integer min_q_m = std::min(q, m); + double *qrdata = new double[m * na]; + double *tau = new double[min_q_m]; + double *work = new double[1]; + integer *p = new integer[na]; + for (int i = 0; i < na; ++i) { + p[i] = 0; + } + + integer info; + + // copy this to qrdata in Lapack convention + for (integer i = 0; i < m; ++i) { + for (integer j = 0; j < n; ++j) { + qrdata[i + m * j] = data[j + n * i]; + } + for (integer j = n; j < na; ++j) { + qrdata[i + m * j] = 0; + } + } + + // 1) Extract householder reflections (useful to compute Q) and R + // m: The number of rows of the matrix A. M >= 0. + // na: The number of columns of the matrix A. N >= 0. + // qrdata: On entry, the M-by-N matrix A. + // m: The leading dimension of the array A. LDA >= max(1,M). + // p: Dimension N + // tau: dimension (min(M,N)) + // work: Internal working array. dimension (3*N) + // info: status + dgeqp3_(&m, &na, qrdata, &m, p, tau, work, &dimWork, &info); + + if (info != 0) { + std::cout << "dgeqp3_:Preparation:" << -info << "th element had an illegal value" << std::endl; + delete[] qrdata; + delete[] work; + delete[] tau; + delete[] p; + throw vpMatrixException::badValue; + } + + dimWork = allocate_work(&work); + + // m: The number of rows of the matrix A. M >= 0. + // na: The number of columns of the matrix A. N >= 0. + // qrdata: On entry, the M-by-N matrix A. + // m: The leading dimension of the array A. LDA >= max(1,M). + // p: Dimension N + // tau: Dimension (min(M,N)) + // work: Internal working array. dimension (3*N) + // info: status + dgeqp3_(&m, &na, qrdata, &m, p, tau, work, &dimWork, &info); + + if (info != 0) { + std::cout << "dgeqp3_:" << -info << " th element had an illegal value" << std::endl; + delete[] qrdata; + delete[] work; + delete[] tau; + delete[] p; + throw vpMatrixException::badValue; + } + + // data now contains the R matrix in its upper triangular (in lapack convention) + // get rank of R in r + na = std::min(n, m); + for (int i = 0; i < na; ++i) { + if (std::abs(qrdata[i + m * i]) < tol) { + --r; + } + } + + // write R + if (squareR) // R r x r + { + R.resize(static_cast(r), static_cast(r)); + for (int i = 0; i < r; ++i) { + for (int j = i; j < r; ++j) { + R[i][j] = qrdata[i + m * j]; + } + } + + // write P + P.resize(static_cast(r), static_cast(n)); + for (int i = 0; i < r; ++i) { + P[i][p[i] - 1] = 1; + } + } + else // R is min(m,n) x n of rank r + { + R.resize(static_cast(na), static_cast(n)); + for (int i = 0; i < na; ++i) { + for (int j = i; j < n; ++j) { + R[i][j] = qrdata[i + m * j]; + } + } + // write P + P.resize(static_cast(n), static_cast(n)); + for (int i = 0; i < n; ++i) { + P[i][p[i] - 1] = 1; + } + } + + // extract Q + // m: The number of rows of the matrix Q. M >= 0. + // q: The number of columns of the matrix Q. M >= N >= 0. + // m: The leading dimension of the array A. LDA >= max(1,M). + // ork: Internal working array. dimension (MAX(1,LWORK)) + // dimWork: The dimension of the array WORK. LWORK >= max(1,N). + // info; status + dorgqr_(&m, &q, &q, qrdata, &m, tau, work, &dimWork, &info); + + // write qrdata into Q + for (int i = 0; i < m; ++i) { + for (int j = 0; j < q; ++j) { + Q[i][j] = qrdata[i + m * j]; + } + } + + delete[] qrdata; + delete[] work; + delete[] tau; + delete[] p; + return static_cast(r); +} +#endif // VISP_HAVE_GSL + +#ifdef VISP_HAVE_GSL +/*! + Compute the QR pivot decomposition of a (m x n) matrix of rank when Lapack and + GSL are available. \param Q : orthogonal matrix (will be modified). \param R : upper-triangular matrix (will be modified). @@ -716,10 +935,8 @@ unsigned int vpMatrix::qr(vpMatrix &Q, vpMatrix &R, bool full, bool squareR, dou \sa qrPivot() */ -unsigned int vpMatrix::qrPivot(vpMatrix &Q, vpMatrix &R, vpMatrix &P, bool full, bool squareR, double tol) const +unsigned int vpMatrix::qrPivotLapackGSL(vpMatrix &Q, vpMatrix &R, vpMatrix &P, bool full, bool squareR, double tol) const { -#if defined(VISP_HAVE_LAPACK) -#if defined(VISP_HAVE_GSL) unsigned int m = rowNum; // also rows of Q unsigned int n = colNum; // also columns of R unsigned int r = std::min(n, m); // a priori non-null rows of R = rank of R @@ -832,151 +1049,79 @@ unsigned int vpMatrix::qrPivot(vpMatrix &Q, vpMatrix &R, vpMatrix &P, bool full, gsl_permutation_free(gsl_p); return r; -#else - integer m = (integer)rowNum; // also rows of Q - integer n = (integer)colNum; // also columns of R - integer r = std::min(n, m); // a priori non-null rows of R = rank of R - integer q = r; // columns of Q and rows of R - integer na = n; // columns of A - - // cannot be full decomposition if m < n - // cannot be full decomposition if m < n - if (full && m > n) { - q = m; // Q is square - na = m; // A is square - } - - // prepare Q and deal with r = 0 - Q.resize(static_cast(m), static_cast(q)); - if (r == 0) { - if (squareR) { - R.resize(0, 0); - P.resize(0, static_cast(n)); - } - else { - R.resize(static_cast(r), static_cast(n)); - P.resize(static_cast(n), static_cast(n)); - } - return 0; - } - - integer dimWork = -1; - integer min_q_m = std::min(q, m); - double *qrdata = new double[m * na]; - double *tau = new double[min_q_m]; - double *work = new double[1]; - integer *p = new integer[na]; - for (int i = 0; i < na; ++i) - p[i] = 0; +} +#endif +#endif - integer info; +/*! + Compute the QR pivot decomposition of a (m x n) matrix of rank r. + Only available if Lapack 3rd party is installed. If Lapack is not installed + we use a Lapack built-in version. - // copy this to qrdata in Lapack convention - for (integer i = 0; i < m; ++i) { - for (integer j = 0; j < n; ++j) - qrdata[i + m * j] = data[j + n * i]; - for (integer j = n; j < na; ++j) - qrdata[i + m * j] = 0; - } + \param Q : orthogonal matrix (will be modified). + \param R : upper-triangular matrix (will be modified). + \param P : the (n x n) permutation matrix. + \param full : whether or not we want full decomposition. + \param squareR : will return only the (r x r) part of R and the (r x n) part of P. + \param tol : tolerance to test the rank of R. - // 1) Extract householder reflections (useful to compute Q) and R - dgeqp3_(&m, // The number of rows of the matrix A. M >= 0. - &na, // The number of columns of the matrix A. N >= 0. - qrdata, /*On entry, the M-by-N matrix A. */ - &m, // The leading dimension of the array A. LDA >= max(1,M). - p, // Dimension N - tau, /*Dimension (min(M,N)) */ - work, // Internal working array. dimension (3*N) - - &dimWork, - &info // status - ); + \return The rank r of the matrix. - if (info != 0) { - std::cout << "dgeqp3_:Preparation:" << -info << "th element had an illegal value" << std::endl; - delete[] qrdata; - delete[] work; - delete[] tau; - delete[] p; - throw vpMatrixException::badValue; - } + If full is false (default) then Q is (m x min(n,m)) and R is (min(n,m) x n). + We then have this.P = Q.R. - dimWork = allocate_work(&work); + If full is true and m > n then Q is (m x m) and R is (n x n). + In this case this.P = Q (R, 0)^T - dgeqp3_(&m, // The number of rows of the matrix A. M >= 0. - &na, // The number of columns of the matrix A. N >= 0. - qrdata, /*On entry, the M-by-N matrix A. */ - &m, // The leading dimension of the array A. LDA >= max(1,M). - p, // Dimension N - tau, /*Dimension (min(M,N)) */ - work, // Internal working array. dimension (3*N) + If squareR is true then R is (r x r) invertible. - &dimWork, - &info // status - ); + Here an example: + \code + #include - if (info != 0) { - std::cout << "dgeqp3_:" << -info << " th element had an illegal value" << std::endl; - delete[] qrdata; - delete[] work; - delete[] tau; - delete[] p; - throw vpMatrixException::badValue; + double residual(vpMatrix M1, vpMatrix M2) + { + return (M1 - M2).frobeniusNorm(); } - // data now contains the R matrix in its upper triangular (in lapack convention) - // get rank of R in r - na = std::min(n, m); - for (int i = 0; i < na; ++i) - if (std::abs(qrdata[i + m * i]) < tol) - r--; - - // write R - if (squareR) // R r x r + int main() { - R.resize(static_cast(r), static_cast(r)); - for (int i = 0; i < r; ++i) - for (int j = i; j < r; ++j) - R[i][j] = qrdata[i + m * j]; + vpMatrix A(4,3); - // write P - P.resize(static_cast(r), static_cast(n)); - for (int i = 0; i < r; ++i) - P[i][p[i] - 1] = 1; - } - else // R is min(m,n) x n of rank r - { - R.resize(static_cast(na), static_cast(n)); - for (int i = 0; i < na; ++i) - for (int j = i; j < n; ++j) - R[i][j] = qrdata[i + m * j]; - // write P - P.resize(static_cast(n), static_cast(n)); - for (int i = 0; i < n; ++i) - P[i][p[i] - 1] = 1; - } + A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/2.; + A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; + A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/4.; + A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/5.; + // A is (4x3) but rank 2 - // extract Q - dorgqr_(&m, // The number of rows of the matrix Q. M >= 0. - &q, // The number of columns of the matrix Q. M >= N >= 0. - &q, qrdata, - &m, // The leading dimension of the array A. LDA >= max(1,M). - tau, - work, // Internal working array. dimension (MAX(1,LWORK)) - &dimWork, // The dimension of the array WORK. LWORK >= max(1,N). - &info // status - ); + // Economic QR (Q 4x3, R 3x3) + vpMatrix Q, R, P; + int r = A.qrPivot(Q, R, P); + std::cout << "A rank: " << r << std::endl; + std::cout << "Residual: " << residual(A*P, Q*R) << std::endl; - // write qrdata into Q - for (int i = 0; i < m; ++i) - for (int j = 0; j < q; ++j) - Q[i][j] = qrdata[i + m * j]; + // Full QR (Q 4x4, R 3x3) + r = A.qrPivot(Q, R, P, true); + std::cout << "QRPivot Residual: " << + residual(A*P, Q.extract(0, 0, 4, 3)*R) << std::endl; - delete[] qrdata; - delete[] work; - delete[] tau; - delete[] p; - return (unsigned int)r; + // Using permutation matrix: keep only non-null part of R + Q.resize(4, r, false); // Q is 4 x 2 + R = R.extract(0, 0, r, 3)*P.t(); // R is 2 x 3 + std::cout << "Full QRPivot Residual: " << + residual(A, Q*R) << std::endl; + } + \endcode + + \sa qrPivot() +*/ +unsigned int vpMatrix::qrPivot(vpMatrix &Q, vpMatrix &R, vpMatrix &P, bool full, bool squareR, double tol) const +{ +#if defined(VISP_HAVE_LAPACK) +#if defined(VISP_HAVE_GSL) + return qrPivotLapackGSL(Q, R, P, full, squareR, tol); +#else + return qrPivotLapack(Q, R, P, full, squareR, tol); #endif #else (void)Q; diff --git a/modules/imgproc/include/visp3/imgproc/vpCircleHoughTransform.h b/modules/imgproc/include/visp3/imgproc/vpCircleHoughTransform.h index aff4746727..a0a0fcf71b 100644 --- a/modules/imgproc/include/visp3/imgproc/vpCircleHoughTransform.h +++ b/modules/imgproc/include/visp3/imgproc/vpCircleHoughTransform.h @@ -667,6 +667,15 @@ class VISP_EXPORT vpCircleHoughTransform typedef vpCircleHoughTransformParams vpCircleHoughTransformParameters; #endif + /** + * \brief Data storage for the computation of the center candidates. + */ + typedef struct vpCenterVotes + { + std::pair m_position; + float m_votes; + } vpCenterVotes; + /** * \brief Construct a new vpCircleHoughTransform object with default parameters. */ @@ -1224,15 +1233,21 @@ class VISP_EXPORT vpCircleHoughTransform */ virtual void computeCenterCandidates(); + /** + * \brief Aggregate center candidates that are close to each other. + * \param[in] peak_positions_votes Vector containing raw center candidates. + */ + virtual void filterCenterCandidates(const std::vector &peak_positions_votes); + /** * \brief Compute the probability of \b circle given the number of pixels voting for * it \b nbVotes. * The probability is defined as the ratio of \b nbVotes by the theoretical number of * pixel that should be visible in the image. * - * @param circle The circle for which we want to evaluate the probability. - * @param nbVotes The number of visible pixels of the given circle. - * @return float The probability of the circle. + * \param[in] circle The circle for which we want to evaluate the probability. + * \param[in] nbVotes The number of visible pixels of the given circle. + * \return float The probability of the circle. */ virtual float computeCircleProbability(const vpImageCircle &circle, const unsigned int &nbVotes); diff --git a/modules/imgproc/src/vpCircleHoughTransform.cpp b/modules/imgproc/src/vpCircleHoughTransform.cpp index 0dadbcc925..a37cfda7b8 100644 --- a/modules/imgproc/src/vpCircleHoughTransform.cpp +++ b/modules/imgproc/src/vpCircleHoughTransform.cpp @@ -65,10 +65,10 @@ void updateAccumulator(const float &x_orig, const float &y_orig, } } -bool sortingCenters(const std::pair, float> &position_vote_a, - const std::pair, float> &position_vote_b) +bool sortingCenters(const vpCircleHoughTransform::vpCenterVotes &position_vote_a, + const vpCircleHoughTransform::vpCenterVotes &position_vote_b) { - return position_vote_a.second > position_vote_b.second; + return position_vote_a.m_votes > position_vote_b.m_votes; } float computeEffectiveRadius(const float &votes, const float &weigthedSumRadius) @@ -555,8 +555,7 @@ vpCircleHoughTransform::computeCenterCandidates() // 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(); - unsigned int nbCols = m_edgeMap.getCols(); + 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 @@ -585,8 +584,7 @@ vpCircleHoughTransform::computeCenterCandidates() int offsetY = minimumYposition; int accumulatorHeight = (maximumYposition - minimumYposition) + 1; if (accumulatorHeight <= 0) { - std::string errMsg("[vpCircleHoughTransform::computeCenterCandidates] Accumulator height <= 0!"); - throw(vpException(vpException::dimensionError, errMsg)); + throw(vpException(vpException::dimensionError, "[vpCircleHoughTransform::computeCenterCandidates] Accumulator height <= 0!")); } vpImage centersAccum(accumulatorHeight, accumulatorWidth + 1, 0.); /*!< Votes for the center candidates.*/ @@ -643,8 +641,7 @@ vpCircleHoughTransform::computeCenterCandidates() rstop = std::min(rmax, rstop); } - float deltar_x = 1.f / std::abs(sx); - float deltar_y = 1.f / std::abs(sy); + 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; @@ -658,8 +655,7 @@ vpCircleHoughTransform::computeCenterCandidates() continue; // It means that the center is outside the search region. } - int x_low, x_high; - int y_low, y_high; + int x_low, x_high, y_low, y_high; if (x1 > 0.) { x_low = static_cast(std::floor(x1)); @@ -693,15 +689,11 @@ vpCircleHoughTransform::computeCenterCandidates() #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, + [](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) + if (((x - offsetX) < 0) || ((x - offsetX) >= nbCols) || + ((y - offsetY) < 0) || ((y - offsetY) >= nbRows) ) { hasToStop = true; } @@ -746,7 +738,7 @@ vpCircleHoughTransform::computeCenterCandidates() int nbColsAccum = centersAccum.getCols(); int nbRowsAccum = centersAccum.getRows(); int nbVotes = -1; - std::vector, float> > peak_positions_votes; + std::vector peak_positions_votes; for (int y = 0; y < nbRowsAccum; ++y) { int left = -1; @@ -762,8 +754,7 @@ vpCircleHoughTransform::computeCenterCandidates() } else if (left >= 0) { int cx = static_cast(((left + x) - 1) * 0.5f); - float sumVotes = 0.; - float x_avg = 0., y_avg = 0.; + 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); @@ -781,7 +772,9 @@ vpCircleHoughTransform::computeCenterCandidates() x_avg /= static_cast(sumVotes); y_avg /= static_cast(sumVotes); std::pair position(y_avg + static_cast(offsetY), x_avg + static_cast(offsetX)); - std::pair, float> position_vote(position, avgVotes); + vpCenterVotes position_vote; + position_vote.m_position = position; + position_vote.m_votes = avgVotes; peak_positions_votes.push_back(position_vote); } if (nbVotes < 0) { @@ -794,14 +787,19 @@ vpCircleHoughTransform::computeCenterCandidates() } } } + 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, float> > merged_peaks_position_votes; + 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].second; + float votes = peak_positions_votes[idPeak].m_votes; if (has_been_merged[idPeak]) { // Ignoring peak that has already been merged continue; @@ -811,15 +809,15 @@ vpCircleHoughTransform::computeCenterCandidates() has_been_merged[idPeak] = true; continue; } - std::pair position = peak_positions_votes[idPeak].first; + std::pair position = peak_positions_votes[idPeak].m_position; std::pair barycenter; - barycenter.first = position.first * peak_positions_votes[idPeak].second; - barycenter.second = position.second * peak_positions_votes[idPeak].second; - float total_votes = peak_positions_votes[idPeak].second; + 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].second; + float votes_candidate = peak_positions_votes[idCandidate].m_votes; if (has_been_merged[idCandidate]) { continue; } @@ -829,7 +827,7 @@ vpCircleHoughTransform::computeCenterCandidates() continue; } // Computing the distance with the peak of insterest - std::pair position_candidate = peak_positions_votes[idCandidate].first; + 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)); @@ -848,15 +846,17 @@ vpCircleHoughTransform::computeCenterCandidates() if (avg_votes > m_algoParams.m_centerMinThresh) { barycenter.first /= total_votes; barycenter.second /= total_votes; - std::pair, float> barycenter_votes(barycenter, avg_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 std::pair, float> &position_vote_a, - const std::pair, float> &position_vote_b) { - return position_vote_a.second > position_vote_b.second; + auto sortingCenters = [](const vpCenterVotes &position_vote_a, + const vpCenterVotes &position_vote_b) { + return position_vote_a.m_votes > position_vote_b.m_votes; }; #endif @@ -866,8 +866,8 @@ vpCircleHoughTransform::computeCenterCandidates() 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].first); - m_centerVotes.push_back(static_cast(merged_peaks_position_votes[i].second)); + m_centerCandidatesList.push_back(merged_peaks_position_votes[i].m_position); + m_centerVotes.push_back(static_cast(merged_peaks_position_votes[i].m_votes)); } } } diff --git a/modules/tracker/me/include/visp3/me/vpMeEllipse.h b/modules/tracker/me/include/visp3/me/vpMeEllipse.h index 9b246c08c7..3b9460cd4b 100644 --- a/modules/tracker/me/include/visp3/me/vpMeEllipse.h +++ b/modules/tracker/me/include/visp3/me/vpMeEllipse.h @@ -563,6 +563,30 @@ class VISP_EXPORT vpMeEllipse : public vpMeTracker */ unsigned int leastSquareRobust(const vpImage &I); + /*! + * Robust least squares method to compute the ellipse to which the vpMeSite + * belong, when we track a circle. Manage also the lists of vpMeSite and corresponding angles, + * and update the expected density of points. + * + * \param um Half width of the image. + * \param vm Half height of the image. + * \param k Count the number of tracked MEs. + * \param w Weights computed by robust estimation. + */ + void leastSquareRobustCircle(const double &um, const double &vm, unsigned int &k, vpColVector &w); + + /*! + * Robust least squares method to compute the ellipse to which the vpMeSite + * belong, when we track an ellipse. Manage also the lists of vpMeSite and corresponding angles, + * and update the expected density of points. + * + * \param um Half width of the image. + * \param vm Half height of the image. + * \param k Count the number of tracked MEs. + * \param w Weights computed by robust estimation. + */ + void leastSquareRobustEllipse(const double &um, const double &vm, unsigned int &k, vpColVector &w); + /*! * Seek along the ellipse or arc of ellipse its two extremities to try * recovering lost points. Try also to complete the parts with no tracked points. diff --git a/modules/tracker/me/src/moving-edges/vpMeEllipse.cpp b/modules/tracker/me/src/moving-edges/vpMeEllipse.cpp index 25c922de56..15e8683f1e 100644 --- a/modules/tracker/me/src/moving-edges/vpMeEllipse.cpp +++ b/modules/tracker/me/src/moving-edges/vpMeEllipse.cpp @@ -645,13 +645,9 @@ void vpMeEllipse::leastSquare(const vpImage &I, const std::vector getParameters(); } -unsigned int vpMeEllipse::leastSquareRobust(const vpImage &I) +void vpMeEllipse::leastSquareRobustCircle(const double &um, const double &vm, unsigned int &k, vpColVector &w) { - double um = I.getWidth() / 2.; - double vm = I.getHeight() / 2.; - const unsigned int nos = numberOfSignal(); - unsigned int k = 0; // count the number of tracked MEs const unsigned int index_0 = 0; const unsigned int index_1 = 1; const unsigned int index_2 = 2; @@ -659,217 +655,239 @@ unsigned int vpMeEllipse::leastSquareRobust(const vpImage &I) const unsigned int index_4 = 4; const unsigned int index_5 = 5; - vpColVector w(nos); - w = 1.0; - // Note that the (nos-k) last rows of w are not used. Hopefully, this is not an issue. + // System A x = b to be solved by least squares + // with A = (u v 1), b = (u^2 + v^2) and x = (2xc, 2yc, r^2-xc^2-yc^2) - if (m_trackCircle) { // we track a circle - // System A x = b to be solved by least squares - // with A = (u v 1), b = (u^2 + v^2) and x = (2xc, 2yc, r^2-xc^2-yc^2) + // Note that the (nos-k) last rows of A, b, xp and yp are not used. + // Hopefully, this is not an issue. + vpMatrix A(nos, 3); + vpColVector b(nos); - // Note that the (nos-k) last rows of A, b, xp and yp are not used. - // Hopefully, this is not an issue. - vpMatrix A(nos, 3); - vpColVector b(nos); - - // Useful to compute the weights in the robust estimation - vpColVector xp(nos), yp(nos); - std::list::const_iterator end = m_meList.end(); - - for (std::list::const_iterator it = m_meList.begin(); it != end; ++it) { - vpMeSite p_me = *it; - if (p_me.getState() == vpMeSite::NO_SUPPRESSION) { - // from (i,j) to (u,v) frame + normalization so that (u,v) in [-1;1] - double u = (p_me.get_jfloat() - um) / um; - double v = (p_me.get_ifloat() - vm) / um; // um to not deform the circle - A[k][index_0] = u; - A[k][index_1] = v; - A[k][index_2] = 1.0; - b[k] = (u * u) + (v * v); - // Useful to compute the weights in the robust estimation - xp[k] = p_me.get_jfloat(); - yp[k] = p_me.get_ifloat(); - - ++k; - } + // Useful to compute the weights in the robust estimation + vpColVector xp(nos), yp(nos); + std::list::const_iterator end = m_meList.end(); + + for (std::list::const_iterator it = m_meList.begin(); it != end; ++it) { + vpMeSite p_me = *it; + if (p_me.getState() == vpMeSite::NO_SUPPRESSION) { + // from (i,j) to (u,v) frame + normalization so that (u,v) in [-1;1] + double u = (p_me.get_jfloat() - um) / um; + double v = (p_me.get_ifloat() - vm) / um; // um to not deform the circle + A[k][index_0] = u; + A[k][index_1] = v; + A[k][index_2] = 1.0; + b[k] = (u * u) + (v * v); + // Useful to compute the weights in the robust estimation + xp[k] = p_me.get_jfloat(); + yp[k] = p_me.get_ifloat(); + + ++k; } + } + + const unsigned int minRequiredNbMe = 3; + if (k < minRequiredNbMe) { + throw(vpException(vpException::dimensionError, "Not enough moving edges %d / %d to track the circle ", + k, m_meList.size())); + } - const unsigned int minRequiredNbMe = 3; - if (k < minRequiredNbMe) { - throw(vpException(vpException::dimensionError, "Not enough moving edges %d / %d to track the circle ", - k, m_meList.size())); + vpRobust r; + r.setMinMedianAbsoluteDeviation(1.0); // Image noise in pixels for the algebraic distance + + unsigned int iter = 0; + double var = 1.0; + vpColVector x(3); + vpMatrix DA(k, 3); + vpColVector Db(k); + vpColVector xg_prev(2); + xg_prev = -10.0; + + // stop after 4 it or if cog variation between 2 it is more than 1 pixel + const unsigned int maxNbIter = 4; + const unsigned int widthDA = DA.getCols(); + while ((iter < maxNbIter) && (var > 0.1)) { + for (unsigned int i = 0; i < k; ++i) { + for (unsigned int j = 0; j < widthDA; ++j) { + DA[i][j] = w[i] * A[i][j]; + } + Db[i] = w[i] * b[i]; } + x = DA.solveBySVD(Db); - vpRobust r; - r.setMinMedianAbsoluteDeviation(1.0); // Image noise in pixels for the algebraic distance + // A circle is a particular ellipse. Going from x for circle to K for ellipse + // using inverse normalization to go back to pixel values + double ratio = vm / um; + m_K[index_0] = (m_K[index_1] = (1.0 / (um * um))); + m_K[index_2] = 0.0; + m_K[index_3] = -(1.0 + (x[index_0] / 2.0)) / um; + m_K[index_4] = -(ratio + (x[index_1] / 2.0)) / um; + m_K[index_5] = -x[index_2] + 1.0 + (ratio * ratio) + x[index_0] + (ratio * x[index_1]); - unsigned int iter = 0; - double var = 1.0; - vpColVector x(3); - vpMatrix DA(k, 3); - vpColVector Db(k); - vpColVector xg_prev(2); - xg_prev = -10.0; - - // stop after 4 it or if cog variation between 2 it is more than 1 pixel - const unsigned int maxNbIter = 4; - const unsigned int widthDA = DA.getCols(); - while ((iter < maxNbIter) && (var > 0.1)) { - for (unsigned int i = 0; i < k; ++i) { - for (unsigned int j = 0; j < widthDA; ++j) { - DA[i][j] = w[i] * A[i][j]; - } - Db[i] = w[i] * b[i]; + getParameters(); + vpColVector xg(2); + xg[0] = m_uc; + xg[1] = m_vc; + var = (xg - xg_prev).frobeniusNorm(); + xg_prev = xg; + + vpColVector residu(k); // near to geometric distance in pixel + for (unsigned int i = 0; i < k; ++i) { + double x = xp[i]; + double y = yp[i]; + double sign = (m_K[index_0] * x * x) + (m_K[index_1] * y * y) + (2. * m_K[index_2] * x * y) + + (2. * m_K[index_3] * x) + (2. * m_K[index_4] * y) + m_K[index_5]; + vpImagePoint ip1, ip2; + ip1.set_uv(x, y); + double ang = computeAngleOnEllipse(ip1); + computePointOnEllipse(ang, ip2); + // residu = 0 if point is exactly on the ellipse, not otherwise + if (sign > 0) { + residu[i] = vpImagePoint::distance(ip1, ip2); } - x = DA.solveBySVD(Db); - - // A circle is a particular ellipse. Going from x for circle to K for ellipse - // using inverse normalization to go back to pixel values - double ratio = vm / um; - m_K[index_0] = (m_K[index_1] = (1.0 / (um * um))); - m_K[index_2] = 0.0; - m_K[index_3] = -(1.0 + (x[index_0] / 2.0)) / um; - m_K[index_4] = -(ratio + (x[index_1] / 2.0)) / um; - m_K[index_5] = -x[index_2] + 1.0 + (ratio * ratio) + x[index_0] + (ratio * x[index_1]); - - getParameters(); - vpColVector xg(2); - xg[0] = m_uc; - xg[1] = m_vc; - var = (xg - xg_prev).frobeniusNorm(); - xg_prev = xg; - - vpColVector residu(k); // near to geometric distance in pixel - for (unsigned int i = 0; i < k; ++i) { - double x = xp[i]; - double y = yp[i]; - double sign = (m_K[index_0] * x * x) + (m_K[index_1] * y * y) + (2. * m_K[index_2] * x * y) - + (2. * m_K[index_3] * x) + (2. * m_K[index_4] * y) + m_K[index_5]; - vpImagePoint ip1, ip2; - ip1.set_uv(x, y); - double ang = computeAngleOnEllipse(ip1); - computePointOnEllipse(ang, ip2); - // residu = 0 if point is exactly on the ellipse, not otherwise - if (sign > 0) { - residu[i] = vpImagePoint::distance(ip1, ip2); - } - else { - residu[i] = -vpImagePoint::distance(ip1, ip2); - } + else { + residu[i] = -vpImagePoint::distance(ip1, ip2); } - r.MEstimator(vpRobust::TUKEY, residu, w); - - ++iter; } + r.MEstimator(vpRobust::TUKEY, residu, w); + + ++iter; } - else { // we track an ellipse +} - // Homogeneous system A x = 0 ; x is the nullspace of A - // K0 u^2 + K1 v^2 + 2 K2 u v + 2 K3 u + 2 K4 v + K5 = 0 - // A = (u^2 v^2 2uv 2u 2v 1), x = (K0 K1 K2 K3 K4 K5)^T +void vpMeEllipse::leastSquareRobustEllipse(const double &um, const double &vm, unsigned int &k, vpColVector &w) +{ + const unsigned int nos = numberOfSignal(); + const unsigned int index_0 = 0; + const unsigned int index_1 = 1; + const unsigned int index_2 = 2; + const unsigned int index_3 = 3; + const unsigned int index_4 = 4; + const unsigned int index_5 = 5; + // Homogeneous system A x = 0 ; x is the nullspace of A + // K0 u^2 + K1 v^2 + 2 K2 u v + 2 K3 u + 2 K4 v + K5 = 0 + // A = (u^2 v^2 2uv 2u 2v 1), x = (K0 K1 K2 K3 K4 K5)^T + + // It would be a bad idea to solve the same system using A x = b where + // A = (u^2 v^2 2uv 2u 2v), b = (-1), x = (K0 K1 K2 K3 K4)^T since it + // cannot consider the case where the origin belongs to the ellipse. + // Another possibility would be to consider K0+K1=1 which is always valid, + // leading to the system A x = b where + // A = (u^2-v^2 2uv 2u 2v 1), b = (-v^2), x = (K0 K2 K3 K4 K5)^T + const unsigned int nbColsA = 6; + vpMatrix A(nos, nbColsA); + // Useful to compute the weights in the robust estimation + vpColVector xp(nos), yp(nos); + std::list::const_iterator end = m_meList.end(); + + for (std::list::const_iterator it = m_meList.begin(); it != end; ++it) { + vpMeSite p_me = *it; + if (p_me.getState() == vpMeSite::NO_SUPPRESSION) { + // from (i,j) to (u,v) frame + normalization so that (u,v) in [-1;1] + double u = (p_me.get_jfloat() - um) / um; + double v = (p_me.get_ifloat() - vm) / vm; + A[k][index_0] = u * u; + A[k][index_1] = v * v; + A[k][index_2] = 2.0 * u * v; + A[k][index_3] = 2.0 * u; + A[k][index_4] = 2.0 * v; + A[k][index_5] = 1.0; + // Useful to compute the weights in the robust estimation + xp[k] = p_me.get_jfloat(); + yp[k] = p_me.get_ifloat(); - // It would be a bad idea to solve the same system using A x = b where - // A = (u^2 v^2 2uv 2u 2v), b = (-1), x = (K0 K1 K2 K3 K4)^T since it - // cannot consider the case where the origin belongs to the ellipse. - // Another possibility would be to consider K0+K1=1 which is always valid, - // leading to the system A x = b where - // A = (u^2-v^2 2uv 2u 2v 1), b = (-v^2), x = (K0 K2 K3 K4 K5)^T + ++k; + } + } + + const unsigned int minRequiredMe = 5; + if (k < minRequiredMe) { + throw(vpException(vpException::dimensionError, "Not enough moving edges to track the ellipse")); + } - vpMatrix A(nos, 6); - // Useful to compute the weights in the robust estimation - vpColVector xp(nos), yp(nos); - std::list::const_iterator end = m_meList.end(); - - for (std::list::const_iterator it = m_meList.begin(); it != end; ++it) { - vpMeSite p_me = *it; - if (p_me.getState() == vpMeSite::NO_SUPPRESSION) { - // from (i,j) to (u,v) frame + normalization so that (u,v) in [-1;1] - double u = (p_me.get_jfloat() - um) / um; - double v = (p_me.get_ifloat() - vm) / vm; - A[k][index_0] = u * u; - A[k][index_1] = v * v; - A[k][index_2] = 2.0 * u * v; - A[k][index_3] = 2.0 * u; - A[k][index_4] = 2.0 * v; - A[k][index_5] = 1.0; - // Useful to compute the weights in the robust estimation - xp[k] = p_me.get_jfloat(); - yp[k] = p_me.get_ifloat(); - - ++k; + vpRobust r; + + r.setMinMedianAbsoluteDeviation(1.0); // image noise in pixels for the geometrical distance + unsigned int iter = 0; + double var = 1.0; + vpMatrix DA(k, 6); + vpMatrix KerDA; + vpColVector xg_prev(2); + xg_prev = -10.0; + + // Stop after 4 iterations or if cog variation between 2 iterations is more than 0.1 pixel + const unsigned int maxIter = 4; + const unsigned int widthDA = DA.getCols(); + while ((iter < maxIter) && (var > 0.1)) { + for (unsigned int i = 0; i < k; ++i) { + for (unsigned int j = 0; j < widthDA; ++j) { + DA[i][j] = w[i] * A[i][j]; } } + unsigned int dim = DA.nullSpace(KerDA, 1); + if (dim > 1) { // case with less than 5 independent points + throw(vpMatrixException(vpMatrixException::rankDeficient, "Linear system for computing the ellipse equation ill conditioned")); + } - const unsigned int minRequiredMe = 5; - if (k < minRequiredMe) { - throw(vpException(vpException::dimensionError, "Not enough moving edges to track the ellipse")); + const unsigned int nparam = 6; + for (unsigned int i = 0; i < nparam; ++i) { + m_K[i] = KerDA[i][0]; // norm(K) = 1 } - vpRobust r; - - r.setMinMedianAbsoluteDeviation(1.0); // image noise in pixels for the geometrical distance - unsigned int iter = 0; - double var = 1.0; - vpMatrix DA(k, 6); - vpMatrix KerDA; - vpColVector xg_prev(2); - xg_prev = -10.0; - - // Stop after 4 iterations or if cog variation between 2 iterations is more than 0.1 pixel - const unsigned int maxIter = 4; - const unsigned int widthDA = DA.getCols(); - while ((iter < maxIter) && (var > 0.1)) { - for (unsigned int i = 0; i < k; ++i) { - for (unsigned int j = 0; j < widthDA; ++j) { - DA[i][j] = w[i] * A[i][j]; - } + // inverse normalization + m_K[index_0] *= vm / um; + m_K[index_1] *= um / vm; + m_K[index_3] = (m_K[index_3] * vm) - (m_K[0] * um) - (m_K[index_2] * vm); + m_K[index_4] = (m_K[index_4] * um) - (m_K[1] * vm) - (m_K[index_2] * um); + m_K[index_5] = (m_K[index_5] * um * vm) - (m_K[index_0] * um * um) - (m_K[index_1] * vm * vm) + - (2.0 * m_K[index_2] * um * vm) - (2.0 * m_K[index_3] * um) - (2.0 * m_K[index_4] * vm); + + getParameters(); // since a, b, and e are used just after + vpColVector xg(2); + xg[0] = m_uc; + xg[1] = m_vc; + var = (xg - xg_prev).frobeniusNorm(); + xg_prev = xg; + + vpColVector residu(k); + for (unsigned int i = 0; i < k; ++i) { + double x = xp[i]; + double y = yp[i]; + double sign = (m_K[0] * x * x) + (m_K[1] * y * y) + (2. * m_K[2] * x * y) + (2. * m_K[3] * x) + (2. * m_K[4] * y) + m_K[5]; + vpImagePoint ip1, ip2; + ip1.set_uv(x, y); + double ang = computeAngleOnEllipse(ip1); + computePointOnEllipse(ang, ip2); + // residu = 0 if point is exactly on the ellipse, not otherwise + if (sign > 0) { + residu[i] = vpImagePoint::distance(ip1, ip2); } - unsigned int dim = DA.nullSpace(KerDA, 1); - if (dim > 1) { // case with less than 5 independent points - throw(vpMatrixException(vpMatrixException::rankDeficient, "Linear system for computing the ellipse equation ill conditioned")); + else { + residu[i] = -vpImagePoint::distance(ip1, ip2); } + } + r.MEstimator(vpRobust::TUKEY, residu, w); - const unsigned int nparam = 6; - for (unsigned int i = 0; i < nparam; ++i) { - m_K[i] = KerDA[i][0]; // norm(K) = 1 - } + ++iter; + } +} - // inverse normalization - m_K[index_0] *= vm / um; - m_K[index_1] *= um / vm; - m_K[index_3] = (m_K[index_3] * vm) - (m_K[0] * um) - (m_K[index_2] * vm); - m_K[index_4] = (m_K[index_4] * um) - (m_K[1] * vm) - (m_K[index_2] * um); - m_K[index_5] = (m_K[index_5] * um * vm) - (m_K[index_0] * um * um) - (m_K[index_1] * vm * vm) - - (2.0 * m_K[index_2] * um * vm) - (2.0 * m_K[index_3] * um) - (2.0 * m_K[index_4] * vm); - - getParameters(); // since a, b, and e are used just after - vpColVector xg(2); - xg[0] = m_uc; - xg[1] = m_vc; - var = (xg - xg_prev).frobeniusNorm(); - xg_prev = xg; - - vpColVector residu(k); - for (unsigned int i = 0; i < k; ++i) { - double x = xp[i]; - double y = yp[i]; - double sign = (m_K[0] * x * x) + (m_K[1] * y * y) + (2. * m_K[2] * x * y) + (2. * m_K[3] * x) + (2. * m_K[4] * y) + m_K[5]; - vpImagePoint ip1, ip2; - ip1.set_uv(x, y); - double ang = computeAngleOnEllipse(ip1); - computePointOnEllipse(ang, ip2); - // residu = 0 if point is exactly on the ellipse, not otherwise - if (sign > 0) { - residu[i] = vpImagePoint::distance(ip1, ip2); - } - else { - residu[i] = -vpImagePoint::distance(ip1, ip2); - } - } - r.MEstimator(vpRobust::TUKEY, residu, w); +unsigned int vpMeEllipse::leastSquareRobust(const vpImage &I) +{ + double um = I.getWidth() / 2.; + double vm = I.getHeight() / 2.; - ++iter; - } + const unsigned int nos = numberOfSignal(); + unsigned int k = 0; // count the number of tracked MEs + + vpColVector w(nos); + w = 1.0; + // Note that the (nos-k) last rows of w are not used. Hopefully, this is not an issue. + + if (m_trackCircle) { // we track a circle + leastSquareRobustCircle(um, vm, k, w); + } + else { // we track an ellipse + leastSquareRobustEllipse(um, vm, k, w); } // end of case ellipse // Remove bad points and outliers from the lists diff --git a/modules/vision/include/visp3/vision/vpPose.h b/modules/vision/include/visp3/vision/vpPose.h index d8b86e1c85..439690f975 100644 --- a/modules/vision/include/visp3/vision/vpPose.h +++ b/modules/vision/include/visp3/vision/vpPose.h @@ -785,6 +785,8 @@ class VISP_EXPORT vpPose int calculArbreDementhon(vpMatrix &b, vpColVector &U, vpHomogeneousMatrix &cMo); private: + void callLagrangePose(vpHomogeneousMatrix &cMo); + //! Define the maximum number of iteration in VVS int vvsIterMax; //! Variable used in the Dementhon approach diff --git a/modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.cpp b/modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.cpp index 6345c445da..6597e80974 100644 --- a/modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.cpp +++ b/modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.cpp @@ -643,23 +643,249 @@ int qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doub return 0; } +bool lmderMostInnerLoop(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int iflag), int m, int n, + double *x, double *fvec, double *fjac, int ldfjac, double ftol, double xtol, unsigned int maxfev, + double *diag, int nprint, int *info, unsigned int *nfev, int *ipvt, + double *qtf, double *wa1, double *wa2, double *wa3, double *wa4, const double &gnorm, int &iter, double &delta, double &par, double &pnorm, + int &iflag, double &fnorm, double &xnorm) +{ + const double tol1 = 0.1, tol5 = 0.5, tol25 = 0.25, tol75 = 0.75, tol0001 = 0.0001; + /* epsmch est la precision machine. */ + const double epsmch = std::numeric_limits::epsilon(); + /* + * debut de la boucle la plus interne. + */ + double ratio = 0.0; + while (ratio < tol0001) { + + /* + * determination du parametre de Levenberg-Marquardt. + */ + lmpar(n, fjac, ldfjac, ipvt, diag, qtf, &delta, &par, wa1, wa2, wa3, wa4); + + /* + * stockage de la direction p et x + p. calcul de la norme de p. + */ + + for (int j = 0; j < n; ++j) { + wa1[j] = -wa1[j]; + wa2[j] = x[j] + wa1[j]; + wa3[j] = diag[j] * wa1[j]; + } + + pnorm = enorm(wa3, n); + + /* + * a la premiere iteration, ajustement de la premiere limite de + * l'etape. + */ + + if (iter == 1) { + delta = vpMath::minimum(delta, pnorm); + } + + /* + * evaluation de la fonction en x + p et calcul de leur norme. + */ + + iflag = 1; + (*ptr_fcn)(m, n, wa2, wa4, fjac, ldfjac, iflag); + + ++(*nfev); + + if (iflag < 0) { + // termination, normal ou imposee par l'utilisateur. + if (iflag < 0) { + *info = iflag; + } + + iflag = 0; + + if (nprint > 0) { + (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag); + } + + return true; + } + + double fnorm1 = enorm(wa4, m); + + /* + * calcul de la reduction reelle mise a l'echelle. + */ + + double actred = -1.0; + + if ((tol1 * fnorm1) < fnorm) { + actred = 1.0 - ((fnorm1 / fnorm) * (fnorm1 / fnorm)); + } + + /* + * calcul de la reduction predite mise a l'echelle et + * de la derivee directionnelle mise a l'echelle. + */ + + for (int i = 0; i < n; ++i) { + wa3[i] = 0.0; + int l = ipvt[i]; + double temp = wa1[l]; + for (int j = 0; j <= i; ++j) { + wa3[j] += *MIJ(fjac, i, j, ldfjac) * temp; + } + } + + double temp1 = enorm(wa3, n) / fnorm; + double temp2 = (sqrt(par) * pnorm) / fnorm; + double prered = (temp1 * temp1) + ((temp2 * temp2) / tol5); + double dirder = -((temp1 * temp1) + (temp2 * temp2)); + + /* + * calcul du rapport entre la reduction reel et predit. + */ + + ratio = 0.0; + + // --in comment: if (prered != 0.0) + if (std::fabs(prered) > std::numeric_limits::epsilon()) { + ratio = actred / prered; + } + + /* + * mise a jour de la limite de l'etape. + */ + + if (ratio > tol25) { + // --comment: if par eq 0.0 or ratio lesseq tol75 + if ((std::fabs(par) <= std::numeric_limits::epsilon()) || (ratio <= tol75)) { + delta = pnorm / tol5; + par *= tol5; + } + } + else { + double temp; + if (actred >= 0.0) { + temp = tol5; + } + else { + temp = (tol5 * dirder) / (dirder + (tol5 * actred)); + } + + if (((tol1 * fnorm1) >= fnorm) || (temp < tol1)) { + temp = tol1; + } + + delta = temp * vpMath::minimum(delta, (pnorm / tol1)); + par /= temp; + } + + /* + * test pour une iteration reussie. + */ + if (ratio >= tol0001) { + /* + * iteration reussie. mise a jour de x, de fvec, et de + * leurs normes. + */ + + for (int j = 0; j < n; ++j) { + x[j] = wa2[j]; + wa2[j] = diag[j] * x[j]; + } + + for (int i = 0; i < m; ++i) { + fvec[i] = wa4[i]; + } + + xnorm = enorm(wa2, n); + fnorm = fnorm1; + ++iter; + } + + /* + * tests pour convergence. + */ + + if ((std::fabs(actred) <= ftol) && (prered <= ftol) && ((tol5 * ratio) <= 1.0)) { + *info = 1; + } + + if (delta <= (xtol * xnorm)) { + *info = 2; + } + + if ((std::fabs(actred) <= ftol) && (prered <= ftol) && ((tol5 * ratio) <= 1.0) && (*info == 2)) { + *info = 3; + } + + if (*info != 0) { + /* + * termination, normal ou imposee par l'utilisateur. + */ + if (iflag < 0) { + *info = iflag; + } + + iflag = 0; + + if (nprint > 0) { + (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag); + } + + return true; + } + /* + * tests pour termination et + * verification des tolerances. + */ + + if (*nfev >= maxfev) { + *info = 5; + } + + if ((std::fabs(actred) <= epsmch) && (prered <= epsmch) && ((tol5 * ratio) <= 1.0)) { + *info = 6; + } + + if (delta <= (epsmch * xnorm)) { + *info = 7; + } + + if (gnorm <= epsmch) { + *info = 8; + } + + if (*info != 0) { + /* + * termination, normal ou imposee par l'utilisateur. + */ + if (iflag < 0) { + *info = iflag; + } + + iflag = 0; + + if (nprint > 0) { + (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag); + } + + return true; + } + } /* fin while ratio >=tol0001 */ + return false; +} + int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int iflag), int m, int n, double *x, double *fvec, double *fjac, int ldfjac, double ftol, double xtol, double gtol, unsigned int maxfev, double *diag, int mode, const double factor, int nprint, int *info, unsigned int *nfev, int *njev, int *ipvt, double *qtf, double *wa1, double *wa2, double *wa3, double *wa4) { - const double tol1 = 0.1, tol5 = 0.5, tol25 = 0.25, tol75 = 0.75, tol0001 = 0.0001; int oncol = TRUE; int iflag, iter; int count = 0; int i, j, l; - double actred, delta, dirder, epsmch, fnorm, fnorm1; - double ratio = std::numeric_limits::epsilon(); - double par, pnorm, prered; - double sum, temp, temp1, temp2, xnorm = 0.0; - - /* epsmch est la precision machine. */ - epsmch = std::numeric_limits::epsilon(); + double delta, fnorm; + double par, pnorm; + double sum, temp, xnorm = 0.0; *info = 0; iflag = 0; @@ -946,224 +1172,12 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, } } - /* - * debut de la boucle la plus interne. - */ - ratio = 0.0; - while (ratio < tol0001) { - - /* - * determination du parametre de Levenberg-Marquardt. - */ - lmpar(n, fjac, ldfjac, ipvt, diag, qtf, &delta, &par, wa1, wa2, wa3, wa4); - - /* - * stockage de la direction p et x + p. calcul de la norme de p. - */ - - for (j = 0; j < n; ++j) { - wa1[j] = -wa1[j]; - wa2[j] = x[j] + wa1[j]; - wa3[j] = diag[j] * wa1[j]; - } - - pnorm = enorm(wa3, n); - - /* - * a la premiere iteration, ajustement de la premiere limite de - * l'etape. - */ - - if (iter == 1) { - delta = vpMath::minimum(delta, pnorm); - } - - /* - * evaluation de la fonction en x + p et calcul de leur norme. - */ - - iflag = 1; - (*ptr_fcn)(m, n, wa2, wa4, fjac, ldfjac, iflag); - - ++(*nfev); - - if (iflag < 0) { - // termination, normal ou imposee par l'utilisateur. - if (iflag < 0) { - *info = iflag; - } - - iflag = 0; - - if (nprint > 0) { - (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag); - } - - return count; - } - - fnorm1 = enorm(wa4, m); - - /* - * calcul de la reduction reelle mise a l'echelle. - */ - - actred = -1.0; - - if ((tol1 * fnorm1) < fnorm) { - actred = 1.0 - ((fnorm1 / fnorm) * (fnorm1 / fnorm)); - } - - /* - * calcul de la reduction predite mise a l'echelle et - * de la derivee directionnelle mise a l'echelle. - */ - - for (i = 0; i < n; ++i) { - wa3[i] = 0.0; - l = ipvt[i]; - temp = wa1[l]; - for (j = 0; j <= i; ++j) { - wa3[j] += *MIJ(fjac, i, j, ldfjac) * temp; - } - } - - temp1 = enorm(wa3, n) / fnorm; - temp2 = (sqrt(par) * pnorm) / fnorm; - prered = (temp1 * temp1) + ((temp2 * temp2) / tol5); - dirder = -((temp1 * temp1) + (temp2 * temp2)); - - /* - * calcul du rapport entre la reduction reel et predit. - */ - - ratio = 0.0; - - // --in comment: if (prered != 0.0) - if (std::fabs(prered) > std::numeric_limits::epsilon()) { - ratio = actred / prered; - } - - /* - * mise a jour de la limite de l'etape. - */ - - if (ratio > tol25) { - // --comment: if par eq 0.0 or ratio lesseq tol75 - if ((std::fabs(par) <= std::numeric_limits::epsilon()) || (ratio <= tol75)) { - delta = pnorm / tol5; - par *= tol5; - } - } - else { - if (actred >= 0.0) { - temp = tol5; - } - else { - temp = (tol5 * dirder) / (dirder + (tol5 * actred)); - } - - if (((tol1 * fnorm1) >= fnorm) || (temp < tol1)) { - temp = tol1; - } - - delta = temp * vpMath::minimum(delta, (pnorm / tol1)); - par /= temp; - } - - /* - * test pour une iteration reussie. - */ - if (ratio >= tol0001) { - /* - * iteration reussie. mise a jour de x, de fvec, et de - * leurs normes. - */ - - for (j = 0; j < n; ++j) { - x[j] = wa2[j]; - wa2[j] = diag[j] * x[j]; - } - - for (i = 0; i < m; ++i) { - fvec[i] = wa4[i]; - } - - xnorm = enorm(wa2, n); - fnorm = fnorm1; - ++iter; - } - - /* - * tests pour convergence. - */ - - if ((std::fabs(actred) <= ftol) && (prered <= ftol) && ((tol5 * ratio) <= 1.0)) { - *info = 1; - } - - if (delta <= (xtol * xnorm)) { - *info = 2; - } - - if ((std::fabs(actred) <= ftol) && (prered <= ftol) && ((tol5 * ratio) <= 1.0) && (*info == 2)) { - *info = 3; - } - - if (*info != 0) { - /* - * termination, normal ou imposee par l'utilisateur. - */ - if (iflag < 0) { - *info = iflag; - } - - iflag = 0; - - if (nprint > 0) { - (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag); - } - - return count; - } - /* - * tests pour termination et - * verification des tolerances. - */ - - if (*nfev >= maxfev) { - *info = 5; - } - - if ((std::fabs(actred) <= epsmch) && (prered <= epsmch) && ((tol5 * ratio) <= 1.0)) { - *info = 6; - } - - if (delta <= (epsmch * xnorm)) { - *info = 7; - } - - if (gnorm <= epsmch) { - *info = 8; - } - - if (*info != 0) { - /* - * termination, normal ou imposee par l'utilisateur. - */ - if (iflag < 0) { - *info = iflag; - } - - iflag = 0; - - if (nprint > 0) { - (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag); - } - - return count; - } - } /* fin while ratio >=tol0001 */ + bool hasFinished = lmderMostInnerLoop(ptr_fcn, m, n, x, fvec, fjac, ldfjac, ftol, xtol, maxfev, + diag, nprint, info, nfev, ipvt, qtf, wa1, wa2, wa3, wa4, gnorm, iter, delta, par, pnorm, + iflag, fnorm, xnorm); + if (hasFinished) { + return count; + } } /*fin while 1*/ return 0; diff --git a/modules/vision/src/pose-estimation/vpPose.cpp b/modules/vision/src/pose-estimation/vpPose.cpp index 6fb7258a7e..e021853c99 100644 --- a/modules/vision/src/pose-estimation/vpPose.cpp +++ b/modules/vision/src/pose-estimation/vpPose.cpp @@ -343,69 +343,68 @@ double vpPose::computeResidual(const vpHomogeneousMatrix &cMo, const vpCameraPar return squared_error; } +void vpPose::callLagrangePose(vpHomogeneousMatrix &cMo) +{ + const int minNbPtLagrangePlan = 4; + const int minNbPtLagrangeNoPlan = 6; + // test if the 3D points are coplanar + double a, b, c, d; // To get the plan coefficients if the points are coplanar + int coplanar_plane_type = 0; + bool plan = coplanar(coplanar_plane_type, &a, &b, &c, &d); + + if (plan == true) { + const int typeCollinear = 4; + if (coplanar_plane_type == typeCollinear) { + throw(vpPoseException(vpPoseException::notEnoughPointError, "Lagrange method cannot be used in that case " + "(points are collinear)")); + } + if (npt < minNbPtLagrangePlan) { + throw(vpPoseException(vpPoseException::notEnoughPointError, + "Lagrange method cannot be used in that case " + "(at least 4 points are required). " + "Not enough point (%d) to compute the pose ", + npt)); + } + poseLagrangePlan(cMo, &plan, &a, &b, &c, &d); + } + else { + if (npt < minNbPtLagrangeNoPlan) { + throw(vpPoseException(vpPoseException::notEnoughPointError, + "Lagrange method cannot be used in that case " + "(at least 6 points are required when 3D points are non coplanar). " + "Not enough point (%d) to compute the pose ", + npt)); + } + poseLagrangeNonPlan(cMo); + } +} + bool vpPose::computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, funcCheckValidityPose func) { const int minNbPtDementhon = 4; const int minNbPtRansac = 4; - const int minNbPtLagrangePlan = 4; - const int minNbPtLagrangeNoPlan = 6; + std::stringstream errMsgDementhon; + errMsgDementhon << "Dementhon method cannot be used in that case " + << "(at least " << minNbPtDementhon << " points are required)" + << "Not enough point (" << npt << ") to compute the pose "; switch (method) { case DEMENTHON: case DEMENTHON_VIRTUAL_VS: case DEMENTHON_LOWE: { if (npt < minNbPtDementhon) { - throw(vpPoseException(vpPoseException::notEnoughPointError, - "Dementhon method cannot be used in that case " - "(at least 4 points are required)" - "Not enough point (%d) to compute the pose ", - npt)); + throw(vpPoseException(vpPoseException::notEnoughPointError, errMsgDementhon.str())); } - // test if the 3D points are coplanar int coplanar_plane_type = 0; bool plan = coplanar(coplanar_plane_type); - if (plan == true) { - poseDementhonPlan(cMo); - } - else { - poseDementhonNonPlan(cMo); - } + plan ? poseDementhonPlan(cMo) : poseDementhonNonPlan(cMo); break; } case LAGRANGE: case LAGRANGE_VIRTUAL_VS: case LAGRANGE_LOWE: { - // test if the 3D points are coplanar - double a, b, c, d; // To get the plan coefficients if the points are coplanar - int coplanar_plane_type = 0; - bool plan = coplanar(coplanar_plane_type, &a, &b, &c, &d); - - if (plan == true) { - const int typeCollinear = 4; - if (coplanar_plane_type == typeCollinear) { - throw(vpPoseException(vpPoseException::notEnoughPointError, "Lagrange method cannot be used in that case " - "(points are collinear)")); - } - if (npt < minNbPtLagrangePlan) { - throw(vpPoseException(vpPoseException::notEnoughPointError, - "Lagrange method cannot be used in that case " - "(at least 4 points are required). " - "Not enough point (%d) to compute the pose ", - npt)); - } - poseLagrangePlan(cMo, &plan, &a, &b, &c, &d); - } - else { - if (npt < minNbPtLagrangeNoPlan) { - throw(vpPoseException(vpPoseException::notEnoughPointError, - "Lagrange method cannot be used in that case " - "(at least 6 points are required when 3D points are non coplanar). " - "Not enough point (%d) to compute the pose ", - npt)); - } - poseLagrangeNonPlan(cMo); - } + callLagrangePose(cMo); break; } case RANSAC: { From abdb1288a2a9f57ee677b5875324a5df51bb58bd Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Tue, 18 Jun 2024 23:14:30 +0200 Subject: [PATCH 08/14] Add missing vpDebug.h header --- modules/ar/src/coin-simulator/vpAR.cpp | 1 + modules/ar/src/coin-simulator/vpSimulator.cpp | 1 + modules/core/include/visp3/core/vpNetwork.h | 5 +- .../vpLinearKalmanFilterInstantiation.cpp | 146 +++++++++--------- modules/core/src/tools/network/vpClient.cpp | 3 + modules/core/src/tools/network/vpNetwork.cpp | 2 + modules/core/src/tools/network/vpServer.cpp | 2 + .../gui/test/display/testVideoDeviceDual.cpp | 17 +- .../src/real-robot/pioneer/vpRobotPioneer.cpp | 3 +- .../src/robot-simulator/vpSimulatorAfma6.cpp | 1 + .../robot-simulator/vpSimulatorViper850.cpp | 1 + .../vpWireFrameSimulator.cpp | 1 + .../framegrabber/1394/vp1394CMUGrabber.cpp | 3 +- .../src/framegrabber/v4l2/vpV4l2Grabber.cpp | 76 +++++---- .../sensor/src/rgb-depth/kinect/vpKinect.cpp | 1 + .../tracker/mbt/src/klt/vpMbKltTracker.cpp | 1 + modules/tracker/mbt/src/vpMbTracker.cpp | 1 + .../tracker/me/src/moving-edges/vpNurbs.cpp | 1 + .../src/pose-estimation/vpPoseFeatures.cpp | 2 + .../feature-builder/vpFeatureBuilderLine.cpp | 1 + .../vpFeatureBuilderPoint3D.cpp | 7 +- 21 files changed, 144 insertions(+), 132 deletions(-) diff --git a/modules/ar/src/coin-simulator/vpAR.cpp b/modules/ar/src/coin-simulator/vpAR.cpp index a2dbafdb69..ea715197a5 100644 --- a/modules/ar/src/coin-simulator/vpAR.cpp +++ b/modules/ar/src/coin-simulator/vpAR.cpp @@ -43,6 +43,7 @@ #ifdef VISP_HAVE_COIN3D_AND_GUI #include +#include #include /* Objets OIV. */ diff --git a/modules/ar/src/coin-simulator/vpSimulator.cpp b/modules/ar/src/coin-simulator/vpSimulator.cpp index 157b736d4b..38909c0541 100644 --- a/modules/ar/src/coin-simulator/vpSimulator.cpp +++ b/modules/ar/src/coin-simulator/vpSimulator.cpp @@ -41,6 +41,7 @@ #ifdef VISP_HAVE_COIN3D_AND_GUI #include +#include #include #include diff --git a/modules/core/include/visp3/core/vpNetwork.h b/modules/core/include/visp3/core/vpNetwork.h index 26ef8e8936..645e00d198 100644 --- a/modules/core/include/visp3/core/vpNetwork.h +++ b/modules/core/include/visp3/core/vpNetwork.h @@ -31,10 +31,11 @@ * TCP Network */ -#ifndef vpNetwork_H -#define vpNetwork_H +#ifndef VP_NETWORK_H +#define VP_NETWORK_H #include +#include #include #include diff --git a/modules/core/src/math/kalman/vpLinearKalmanFilterInstantiation.cpp b/modules/core/src/math/kalman/vpLinearKalmanFilterInstantiation.cpp index e78d58ccf8..9263e95b17 100644 --- a/modules/core/src/math/kalman/vpLinearKalmanFilterInstantiation.cpp +++ b/modules/core/src/math/kalman/vpLinearKalmanFilterInstantiation.cpp @@ -55,7 +55,7 @@ BEGIN_VISP_NAMESPACE initStateConstVelWithColoredNoise_MeasureVel(), initStateConstAccWithColoredNoise_MeasureVel(). - \warning It is requiered to set the state model before using this method. + \warning It is required to set the state model before using this method. \param n_signal : Number of signal to filter. @@ -78,7 +78,7 @@ BEGIN_VISP_NAMESPACE second. Depending on the filter modelization, this value may not be used. This is for example the case for the vpLinearKalmanFilterInstantiation::stateConstVelWithColoredNoise_MeasureVel -model implemented in initStateConstVelWithColoredNoise_MeasureVel(). + model implemented in initStateConstVelWithColoredNoise_MeasureVel(). \exception vpException::badValue : Bad rho value wich is not in [0:1[. @@ -89,24 +89,24 @@ model implemented in initStateConstVelWithColoredNoise_MeasureVel(). dimensional signal. \code -#include - -int main() -{ - vpLinearKalmanFilterInstantiation kalman; - // Select a constant velocity state model with colored noise - // Measures are velocities - kalman.setStateModel(vpLinearKalmanFilterInstantiation::stateConstVelWithColoredNoise_MeasureVel); - - // Initialise the filter for a one dimension signal - int signal = 1; - vpColVector sigma_state(2); // State vector size is 2 - vpColVector sigma_measure(1); // Measure vector size is 1 - double rho = 0.9; - double dummy = 0; // non used parameter for the selected state model - - kalman.initFilter(signal, sigma_state, sigma_measure, rho, dummy); -} + #include + + int main() + { + vpLinearKalmanFilterInstantiation kalman; + // Select a constant velocity state model with colored noise + // Measures are velocities + kalman.setStateModel(vpLinearKalmanFilterInstantiation::stateConstVelWithColoredNoise_MeasureVel); + + // Initialise the filter for a one dimension signal + int signal = 1; + vpColVector sigma_state(2); // State vector size is 2 + vpColVector sigma_measure(1); // Measure vector size is 1 + double rho = 0.9; + double dummy = 0; // non used parameter for the selected state model + + kalman.initFilter(signal, sigma_state, sigma_measure, rho, dummy); + } \endcode The example below shows a more complete example to filter a two @@ -114,62 +114,62 @@ int main() velocities from velocity measures. \code -#include - -int main() -{ - vpLinearKalmanFilterInstantiation kalman; - - // Set the constant velocity state model used for the filtering - vpLinearKalmanFilterInstantiation::vpStateModel model; - model = vpLinearKalmanFilterInstantiation::stateConstVelWithColoredNoise_MeasureVel; - kalman.setStateModel(model); - - // We are now able to retrieve the size of the state vector and measure vector - int state_size = kalman.getStateSize(); - - // Filter the x and y velocities of a target (2 signals are to consider) - int nsignal = 2; - - // Initialize the filter parameters: - // - Firstly, the state variance - int size = state_size*nsignal; - vpColVector sigma_state(size); - sigma_state[1] = 0.001; // Variance on the acceleration for the 1st signal (x) - sigma_state[3] = 0.002; // Variance on the acceleration for the 2nd signal (y) - // - Secondly, the measures variance - vpColVector sigma_measure(nsignal); // 2 velocity measures available - sigma_measure[0] = 0.03; // Variance on the x velocity measure - sigma_measure[1] = 0.06; // Variance on the y velocity measure - // - Thirdly, the correlation between successive accelerations - double rho = 0.9; - - double dummy = 0; // non used parameter for the selected state model - - // Initialize the filter - // The state model was set before - kalman.initFilter(nsignal, sigma_state, sigma_measure, rho, dummy); - - // Does the filtering - vpColVector vm(2); // Measured velocities - for ( ; ; ) { - // Get the two dimensional velocity measures - // vm[0] = ...; - // vm[1] = ...; - - // Compute the filtering and the prediction - kalman.filter(vm); - // Print the estimation of the velocities (1st value of the state vector) - std::cout << "Estimated x velocity: kalman.Xest[0]" << std::endl; - std::cout << "Estimated y velocity: kalman.Xest[kalman.getStateSize()]" - << std::endl; - // The one step prediction is available in kalman.Xpre variable + #include + + int main() + { + vpLinearKalmanFilterInstantiation kalman; + + // Set the constant velocity state model used for the filtering + vpLinearKalmanFilterInstantiation::vpStateModel model; + model = vpLinearKalmanFilterInstantiation::stateConstVelWithColoredNoise_MeasureVel; + kalman.setStateModel(model); + + // We are now able to retrieve the size of the state vector and measure vector + int state_size = kalman.getStateSize(); + + // Filter the x and y velocities of a target (2 signals are to consider) + int nsignal = 2; + + // Initialize the filter parameters: + // - Firstly, the state variance + int size = state_size*nsignal; + vpColVector sigma_state(size); + sigma_state[1] = 0.001; // Variance on the acceleration for the 1st signal (x) + sigma_state[3] = 0.002; // Variance on the acceleration for the 2nd signal (y) + // - Secondly, the measures variance + vpColVector sigma_measure(nsignal); // 2 velocity measures available + sigma_measure[0] = 0.03; // Variance on the x velocity measure + sigma_measure[1] = 0.06; // Variance on the y velocity measure + // - Thirdly, the correlation between successive accelerations + double rho = 0.9; + + double dummy = 0; // non used parameter for the selected state model + + // Initialize the filter + // The state model was set before + kalman.initFilter(nsignal, sigma_state, sigma_measure, rho, dummy); + + // Does the filtering + vpColVector vm(2); // Measured velocities + for ( ; ; ) { + // Get the two dimensional velocity measures + // vm[0] = ...; + // vm[1] = ...; + + // Compute the filtering and the prediction + kalman.filter(vm); + // Print the estimation of the velocities (1st value of the state vector) + std::cout << "Estimated x velocity: kalman.Xest[0]" << std::endl; + std::cout << "Estimated y velocity: kalman.Xest[kalman.getStateSize()]" + << std::endl; + // The one step prediction is available in kalman.Xpre variable + } } -} \endcode */ -void vpLinearKalmanFilterInstantiation::initFilter(unsigned int n_signal, vpColVector &sigma_state, - vpColVector &sigma_measure, double rho, double delta_t) + void vpLinearKalmanFilterInstantiation::initFilter(unsigned int n_signal, vpColVector &sigma_state, + vpColVector &sigma_measure, double rho, double delta_t) { switch (model) { case stateConstVelWithColoredNoise_MeasureVel: diff --git a/modules/core/src/tools/network/vpClient.cpp b/modules/core/src/tools/network/vpClient.cpp index 93fdc6f22e..c57366dd24 100644 --- a/modules/core/src/tools/network/vpClient.cpp +++ b/modules/core/src/tools/network/vpClient.cpp @@ -40,6 +40,9 @@ // inet_ntop() not supported on win XP #ifdef VISP_HAVE_FUNC_INET_NTOP + +#include + BEGIN_VISP_NAMESPACE vpClient::vpClient() : vpNetwork(), m_numberOfAttempts(0) { } diff --git a/modules/core/src/tools/network/vpNetwork.cpp b/modules/core/src/tools/network/vpNetwork.cpp index 8419f05e97..77feb13bd2 100644 --- a/modules/core/src/tools/network/vpNetwork.cpp +++ b/modules/core/src/tools/network/vpNetwork.cpp @@ -40,6 +40,8 @@ // inet_ntop() not supported on win XP #ifdef VISP_HAVE_FUNC_INET_NTOP + +#include BEGIN_VISP_NAMESPACE vpNetwork::vpNetwork() : emitter(), receptor_list(), readFileDescriptor(), socketMax(0), request_list(), max_size_message(999999), diff --git a/modules/core/src/tools/network/vpServer.cpp b/modules/core/src/tools/network/vpServer.cpp index c1e62cfab1..b8f23c8663 100644 --- a/modules/core/src/tools/network/vpServer.cpp +++ b/modules/core/src/tools/network/vpServer.cpp @@ -40,6 +40,8 @@ // inet_ntop() not supported on win XP #ifdef VISP_HAVE_FUNC_INET_NTOP +#include + #if defined(__APPLE__) && defined(__MACH__) // Apple OSX and iOS (Darwin) #include // To detect OSX or IOS using TARGET_OS_IPHONE or TARGET_OS_IOS macro #endif diff --git a/modules/gui/test/display/testVideoDeviceDual.cpp b/modules/gui/test/display/testVideoDeviceDual.cpp index 5fa5dd5a6c..faab10944f 100644 --- a/modules/gui/test/display/testVideoDeviceDual.cpp +++ b/modules/gui/test/display/testVideoDeviceDual.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,12 +29,12 @@ * * Description: * Test for image display. - * -*****************************************************************************/ + */ #include #include #include +#include #include #include #include @@ -261,9 +260,9 @@ int main(int argc, const char **argv) std::cout << " No display is available\n"; } return EXIT_FAILURE; - } + } - // Create 2 images + // Create 2 images vpImage I1(240, 320), I2(240, 320); I1 = 128; I2 = 255; @@ -352,12 +351,12 @@ int main(int argc, const char **argv) delete d1; delete d2; return EXIT_SUCCESS; -} + } catch (const vpException &e) { std::cout << "Catch an exception: " << e << std::endl; return EXIT_FAILURE; } - } +} #else int main() { vpERROR_TRACE("You do not have display functionalities..."); } diff --git a/modules/robot/src/real-robot/pioneer/vpRobotPioneer.cpp b/modules/robot/src/real-robot/pioneer/vpRobotPioneer.cpp index 1d021e3a37..f98200b396 100644 --- a/modules/robot/src/real-robot/pioneer/vpRobotPioneer.cpp +++ b/modules/robot/src/real-robot/pioneer/vpRobotPioneer.cpp @@ -42,6 +42,7 @@ // This error is due to cmath header included from vpMath.h that makes // isfinite() ambiguous between ::isfinite() and std::isfinite() #include +#include #ifdef VISP_HAVE_PIONEER @@ -49,7 +50,7 @@ BEGIN_VISP_NAMESPACE /*! Default constructor that initializes Aria. */ -vpRobotPioneer::vpRobotPioneer() : vpPioneer(), ArRobot() + vpRobotPioneer::vpRobotPioneer() : vpPioneer(), ArRobot() { isInitialized = false; diff --git a/modules/robot/src/robot-simulator/vpSimulatorAfma6.cpp b/modules/robot/src/robot-simulator/vpSimulatorAfma6.cpp index b0172373dd..3dddc7c3e7 100644 --- a/modules/robot/src/robot-simulator/vpSimulatorAfma6.cpp +++ b/modules/robot/src/robot-simulator/vpSimulatorAfma6.cpp @@ -38,6 +38,7 @@ #include // std::fabs #include // numeric_limits #include +#include #include #include #include diff --git a/modules/robot/src/robot-simulator/vpSimulatorViper850.cpp b/modules/robot/src/robot-simulator/vpSimulatorViper850.cpp index f97607b3a3..86ebb52249 100644 --- a/modules/robot/src/robot-simulator/vpSimulatorViper850.cpp +++ b/modules/robot/src/robot-simulator/vpSimulatorViper850.cpp @@ -40,6 +40,7 @@ #include // std::fabs #include // numeric_limits #include +#include #include #include #include diff --git a/modules/robot/src/wireframe-simulator/vpWireFrameSimulator.cpp b/modules/robot/src/wireframe-simulator/vpWireFrameSimulator.cpp index dacb00a618..dca6da6b38 100644 --- a/modules/robot/src/wireframe-simulator/vpWireFrameSimulator.cpp +++ b/modules/robot/src/wireframe-simulator/vpWireFrameSimulator.cpp @@ -60,6 +60,7 @@ #include "vpView.h" #include "vpVwstack.h" +#include #include #include #include diff --git a/modules/sensor/src/framegrabber/1394/vp1394CMUGrabber.cpp b/modules/sensor/src/framegrabber/1394/vp1394CMUGrabber.cpp index 255db1e089..25e3c83554 100644 --- a/modules/sensor/src/framegrabber/1394/vp1394CMUGrabber.cpp +++ b/modules/sensor/src/framegrabber/1394/vp1394CMUGrabber.cpp @@ -39,6 +39,7 @@ #include +#include #include #include @@ -46,7 +47,7 @@ BEGIN_VISP_NAMESPACE /*! Basic constructor. */ -vp1394CMUGrabber::vp1394CMUGrabber() + vp1394CMUGrabber::vp1394CMUGrabber() : index(0), // If a camera was not selected the first one (index = 0) will // be used _format(-1), _mode(-1), _fps(-1), _modeauto(true), _gain(0), _shutter(0), _color(vp1394CMUGrabber::UNKNOWN) diff --git a/modules/sensor/src/framegrabber/v4l2/vpV4l2Grabber.cpp b/modules/sensor/src/framegrabber/v4l2/vpV4l2Grabber.cpp index b761eb2322..b7ab43fbb5 100644 --- a/modules/sensor/src/framegrabber/v4l2/vpV4l2Grabber.cpp +++ b/modules/sensor/src/framegrabber/v4l2/vpV4l2Grabber.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Framegrabber based on Video4Linux2 driver. - * -*****************************************************************************/ + */ /*! \file vpV4l2Grabber.cpp @@ -54,6 +52,7 @@ #include #include +#include #include #include //#include @@ -89,10 +88,10 @@ const unsigned int vpV4l2Grabber::FRAME_SIZE = 288; fps. These framerates are reachable only if enought buffers are set. - Input board: vpV4l2Grabber::DEFAULT_INPUT. Video input port. Use -setInput() to change it. + setInput() to change it. - Image size acquisition: vpV4l2Grabber::DEFAULT_SCALE. Use either -setScale() or setWidth() and setHeight() to change it. + setScale() or setWidth() and setHeight() to change it. \code vpImage I; // Grey level image @@ -108,40 +107,39 @@ setScale() or setWidth() and setHeight() to change it. \endcode The grabber allows also to grab a portion of an image using a region of -interest. The following example shows how to grab a 320x240 region located on -the top/left corner of the image that has a higher resolution (ie 640x480). + interest. The following example shows how to grab a 320x240 region located on + the top/left corner of the image that has a higher resolution (ie 640x480). \code -#include -#include -#include + #include + #include + #include -int main() -{ -#if defined(VISP_HAVE_V4L2) && defined(VISP_HAVE_X11) - vpDisplayX *d; - vpImage I; - vpRect roi(0, 0, 320, 240); // specify the region to crop + int main() + { + #if defined(VISP_HAVE_V4L2) && defined(VISP_HAVE_X11) + vpDisplayX *d; + vpImage I; + vpRect roi(0, 0, 320, 240); // specify the region to crop - vpV4l2Grabber g; + vpV4l2Grabber g; - bool first = true; - while (1) { - g.acquire(I, roi); - if(first) { - d = new vpDisplayX(I); - first = false; + bool first = true; + while (1) { + g.acquire(I, roi); + if(first) { + d = new vpDisplayX(I); + first = false; + } + vpDisplay::display(I); + vpDisplay::flush(I); + if (vpDisplay::getClick(I, false)) + break; } - vpDisplay::display(I); - vpDisplay::flush(I); - if (vpDisplay::getClick(I, false)) - break; - } - vpImageIo::write(I, "image.pgm"); // Save the last image - delete d; -#endif + vpImageIo::write(I, "image.pgm"); // Save the last image + delete d; + #endif \endcode - */ vpV4l2Grabber::vpV4l2Grabber() : fd(-1), device(), cap(), streamparm(), inp(nullptr), std(nullptr), fmt(nullptr), ctl(nullptr), fmt_v4l2(), fmt_me(), reqbufs(), @@ -179,10 +177,10 @@ vpV4l2Grabber::vpV4l2Grabber() fps. These framerates are reachable only if enought buffers are set. - Input board: vpV4l2Grabber::DEFAULT_INPUT. Video input port. Use - setInput() to change it. + setInput() to change it. - Image size acquisition: vpV4l2Grabber::DEFAULT_SCALE. Use either - setScale() or setWidth() and setHeight to change it. + setScale() or setWidth() and setHeight to change it. \code vpImage I; // Grey level image @@ -195,7 +193,6 @@ vpV4l2Grabber::vpV4l2Grabber() g.open(I); // Open the grabber g.acquire(I); // Acquire a 768x576 grey image - \endcode */ @@ -242,7 +239,6 @@ vpV4l2Grabber::vpV4l2Grabber(bool verbose) g.open(I); // Open the grabber g.acquire(I); // Acquire a 384x288 grey image - \endcode */ vpV4l2Grabber::vpV4l2Grabber(unsigned input, unsigned scale) @@ -279,7 +275,7 @@ vpV4l2Grabber::vpV4l2Grabber(unsigned input, unsigned scale) set the number of buffers to 3. - Framerate acquisition: 25 fps. Use setFramerate() to set 25 fps or 50 - fps. These framerates are reachable only if enought buffers are set. + fps. These framerates are reachable only if enough buffers are set. \code vpImage I; // Grey level image @@ -336,9 +332,7 @@ vpV4l2Grabber::vpV4l2Grabber(vpImage &I, unsigned input, unsigned // images and open the grabber g.acquire(I); // Acquire a 384x288 color image - \endcode - */ vpV4l2Grabber::vpV4l2Grabber(vpImage &I, unsigned input, unsigned scale) : fd(-1), device(), cap(), streamparm(), inp(nullptr), std(nullptr), fmt(nullptr), ctl(nullptr), fmt_v4l2(), fmt_me(), reqbufs(), diff --git a/modules/sensor/src/rgb-depth/kinect/vpKinect.cpp b/modules/sensor/src/rgb-depth/kinect/vpKinect.cpp index fd48e8a8b9..225aba1364 100644 --- a/modules/sensor/src/rgb-depth/kinect/vpKinect.cpp +++ b/modules/sensor/src/rgb-depth/kinect/vpKinect.cpp @@ -44,6 +44,7 @@ #include // numeric_limits +#include #include #include diff --git a/modules/tracker/mbt/src/klt/vpMbKltTracker.cpp b/modules/tracker/mbt/src/klt/vpMbKltTracker.cpp index 7f7ba01789..1f06395fb2 100644 --- a/modules/tracker/mbt/src/klt/vpMbKltTracker.cpp +++ b/modules/tracker/mbt/src/klt/vpMbKltTracker.cpp @@ -34,6 +34,7 @@ *****************************************************************************/ #include +#include #include #include #include diff --git a/modules/tracker/mbt/src/vpMbTracker.cpp b/modules/tracker/mbt/src/vpMbTracker.cpp index 60218a3177..715cc3f17c 100644 --- a/modules/tracker/mbt/src/vpMbTracker.cpp +++ b/modules/tracker/mbt/src/vpMbTracker.cpp @@ -49,6 +49,7 @@ #endif #include +#include #include #include #include diff --git a/modules/tracker/me/src/moving-edges/vpNurbs.cpp b/modules/tracker/me/src/moving-edges/vpNurbs.cpp index ffd7d2664d..9ec3a1cb6e 100644 --- a/modules/tracker/me/src/moving-edges/vpNurbs.cpp +++ b/modules/tracker/me/src/moving-edges/vpNurbs.cpp @@ -34,6 +34,7 @@ #include // std::fabs #include // numeric_limits #include +#include #include BEGIN_VISP_NAMESPACE diff --git a/modules/vision/src/pose-estimation/vpPoseFeatures.cpp b/modules/vision/src/pose-estimation/vpPoseFeatures.cpp index 1033a7fd5b..f52086f3c7 100644 --- a/modules/vision/src/pose-estimation/vpPoseFeatures.cpp +++ b/modules/vision/src/pose-estimation/vpPoseFeatures.cpp @@ -30,6 +30,8 @@ * Description: * Pose computation from any features. */ + +#include #include #if defined(VISP_HAVE_MODULE_VISUAL_FEATURES) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) diff --git a/modules/visual_features/src/feature-builder/vpFeatureBuilderLine.cpp b/modules/visual_features/src/feature-builder/vpFeatureBuilderLine.cpp index 129d8743fe..7000547b95 100644 --- a/modules/visual_features/src/feature-builder/vpFeatureBuilderLine.cpp +++ b/modules/visual_features/src/feature-builder/vpFeatureBuilderLine.cpp @@ -39,6 +39,7 @@ and visual feature Line */ +#include #include #include diff --git a/modules/visual_features/src/feature-builder/vpFeatureBuilderPoint3D.cpp b/modules/visual_features/src/feature-builder/vpFeatureBuilderPoint3D.cpp index ea62ffce75..049b8b1f91 100644 --- a/modules/visual_features/src/feature-builder/vpFeatureBuilderPoint3D.cpp +++ b/modules/visual_features/src/feature-builder/vpFeatureBuilderPoint3D.cpp @@ -38,6 +38,8 @@ \brief conversion between tracker and visual feature 3D Point */ + +#include #include #include #include @@ -72,8 +74,3 @@ void vpFeatureBuilder::create(vpFeaturePoint3D &s, const vpPoint &t) } } END_VISP_NAMESPACE -/* - * Local variables: - * c-basic-offset: 2 - * End: - */ From 24c0770d0f6f4b009a889be96d77ecbb310a05ce Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Wed, 19 Jun 2024 13:33:04 +0200 Subject: [PATCH 09/14] Remove usage of Panda3D macros in ar module, resolve typedef to function pointer, usage as function parameter is still incorrect --- .../include/visp3/ar/vpPanda3DBaseRenderer.h | 6 ++-- .../visp3/ar/vpPanda3DPostProcessFilter.h | 2 +- .../include/visp3/ar/vpPanda3DRGBRenderer.h | 2 +- .../vpPanda3DBaseRenderer.cpp | 6 ++-- .../vpPanda3DRendererSet.cpp | 4 +-- .../generator/visp_python_bindgen/header.py | 4 +-- .../visp_python_bindgen/header_utils.py | 2 +- .../generator/visp_python_bindgen/utils.py | 28 ++++++++++++++++--- 8 files changed, 37 insertions(+), 17 deletions(-) diff --git a/modules/ar/include/visp3/ar/vpPanda3DBaseRenderer.h b/modules/ar/include/visp3/ar/vpPanda3DBaseRenderer.h index bd02273272..2c69edb298 100644 --- a/modules/ar/include/visp3/ar/vpPanda3DBaseRenderer.h +++ b/modules/ar/include/visp3/ar/vpPanda3DBaseRenderer.h @@ -78,7 +78,7 @@ class VISP_EXPORT vpPanda3DBaseRenderer * @param framework * @param window */ - void initFromParent(std::shared_ptr framework, PT(WindowFramework) window); + void initFromParent(std::shared_ptr framework, std::shared_ptr window); virtual void renderFrame(); @@ -281,10 +281,10 @@ class VISP_EXPORT vpPanda3DBaseRenderer const std::string m_name; //! name of the renderer int m_renderOrder; //! Rendering priority for this renderer and its buffers. A lower value will be rendered first. Should be used when calling make_output in setupRenderTarget() std::shared_ptr m_framework; //! Pointer to the active panda framework - PT(WindowFramework) m_window; //! Pointer to owning window, which can create buffers etc. It is not necessarily visible. + std::shared_ptr m_window; //! Pointer to owning window, which can create buffers etc. It is not necessarily visible. vpPanda3DRenderParameters m_renderParameters; //! Rendering parameters NodePath m_renderRoot; //! Node containing all the objects and the camera for this renderer - PT(Camera) m_camera; + PointerTo m_camera; NodePath m_cameraPath; //! NodePath of the camera std::vector m_buffers; //! Set of buffers that this renderer uses. This storage contains weak refs to those buffers and should not deallocate them. }; diff --git a/modules/ar/include/visp3/ar/vpPanda3DPostProcessFilter.h b/modules/ar/include/visp3/ar/vpPanda3DPostProcessFilter.h index c5986d2379..4e13f8c7c5 100644 --- a/modules/ar/include/visp3/ar/vpPanda3DPostProcessFilter.h +++ b/modules/ar/include/visp3/ar/vpPanda3DPostProcessFilter.h @@ -87,7 +87,7 @@ class VISP_EXPORT vpPanda3DPostProcessFilter : public vpPanda3DBaseRenderer std::shared_ptr m_inputRenderer; bool m_isOutput; //! Whether this filter is an output to be used and should be copied to ram std::string m_fragmentShader; - PT(Shader) m_shader; + PointerTo m_shader; Texture *m_texture; GraphicsOutput *m_buffer; diff --git a/modules/ar/include/visp3/ar/vpPanda3DRGBRenderer.h b/modules/ar/include/visp3/ar/vpPanda3DRGBRenderer.h index 8132974df2..f847bfece6 100644 --- a/modules/ar/include/visp3/ar/vpPanda3DRGBRenderer.h +++ b/modules/ar/include/visp3/ar/vpPanda3DRGBRenderer.h @@ -111,7 +111,7 @@ class VISP_EXPORT vpPanda3DRGBRenderer : public vpPanda3DBaseRenderer, public vp NodePath m_backgroundImage; DisplayRegion *m_display2d; - PT(Texture) m_backgroundTexture; + Texture* m_backgroundTexture; }; diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DBaseRenderer.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DBaseRenderer.cpp index 5fb8d5b285..13e9a6f9f0 100644 --- a/modules/ar/src/panda3d-simulator/vpPanda3DBaseRenderer.cpp +++ b/modules/ar/src/panda3d-simulator/vpPanda3DBaseRenderer.cpp @@ -57,11 +57,11 @@ void vpPanda3DBaseRenderer::initFramework() WindowProperties winProps; winProps.set_size(LVecBase2i(m_renderParameters.getImageWidth(), m_renderParameters.getImageHeight())); int flags = GraphicsPipe::BF_refuse_window; - m_window = m_framework->open_window(winProps, flags); + m_window = std::shared_ptr(m_framework->open_window(winProps, flags)); // try and reopen with visible window if (m_window == nullptr) { winProps.set_minimized(true); - m_window = m_framework->open_window(winProps, 0); + m_window = std::shared_ptr(m_framework->open_window(winProps, 0)); } if (m_window == nullptr) { throw vpException(vpException::notInitialized, @@ -74,7 +74,7 @@ void vpPanda3DBaseRenderer::initFramework() //m_window->get_display_region_3d()->set_camera(m_cameraPath); } -void vpPanda3DBaseRenderer::initFromParent(std::shared_ptr framework, PT(WindowFramework) window) +void vpPanda3DBaseRenderer::initFromParent(std::shared_ptr framework, std::shared_ptr window) { m_framework = framework; m_window = window; diff --git a/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp b/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp index 3f1e0849a6..10ed67cdff 100644 --- a/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp +++ b/modules/ar/src/panda3d-simulator/vpPanda3DRendererSet.cpp @@ -59,10 +59,10 @@ void vpPanda3DRendererSet::initFramework() WindowProperties winProps; winProps.set_size(LVecBase2i(m_renderParameters.getImageWidth(), m_renderParameters.getImageHeight())); int flags = GraphicsPipe::BF_refuse_window; - m_window = m_framework->open_window(winProps, flags); + m_window = std::shared_ptr(m_framework->open_window(winProps, flags)); if (m_window == nullptr) { winProps.set_minimized(true); - m_window = m_framework->open_window(winProps, 0); + m_window = std::shared_ptr(m_framework->open_window(winProps, 0)); } if (m_window == nullptr) { throw vpException(vpException::fatalError, "Could not open Panda3D window (hidden or visible)"); diff --git a/modules/python/generator/visp_python_bindgen/header.py b/modules/python/generator/visp_python_bindgen/header.py index 4675bb76b5..9ce9702cec 100644 --- a/modules/python/generator/visp_python_bindgen/header.py +++ b/modules/python/generator/visp_python_bindgen/header.py @@ -438,7 +438,7 @@ def add_method_doc_to_pyargs(method: types.Method, py_arg_strs: List[str]) -> Li method_spec_dict = OrderedDict(k for k in zip(method_template_names, method_spec)) new_specs.update(method_spec_dict) method_str, method_data = define_method(method, method_config, True, - new_specs, self, header_env, class_def_names) + new_specs, self, header_env, class_def_names) add_to_method_dict(method_data.py_name, MethodBinding(method_str, is_static=method.static, is_lambda=f'{name_cpp}::*' not in method_str, is_operator=False, is_constructor=False, @@ -446,7 +446,7 @@ def add_method_doc_to_pyargs(method: types.Method, py_arg_strs: List[str]) -> Li generated_methods.append(method_data) else: method_str, method_data = define_method(method, method_config, True, - owner_specs, self, header_env, class_def_names) + owner_specs, self, header_env, class_def_names) add_to_method_dict(method_data.py_name, MethodBinding(method_str, is_static=method.static, is_lambda=f'{name_cpp}::*' not in method_str, is_operator=False, is_constructor=False, diff --git a/modules/python/generator/visp_python_bindgen/header_utils.py b/modules/python/generator/visp_python_bindgen/header_utils.py index d31750556a..6f015a9f24 100644 --- a/modules/python/generator/visp_python_bindgen/header_utils.py +++ b/modules/python/generator/visp_python_bindgen/header_utils.py @@ -110,7 +110,7 @@ def build_naive_mapping(self, data: Union[NamespaceScope, ClassScope], mapping, current_mapping[alias.alias] = get_type(alias.type, {}, current_mapping) for typedef in data.typedefs: - if not name_is_anonymous(typedef.type.typename): + if not typedef_is_anonymous(typedef.type): current_mapping[typedef.name] = get_type(typedef.type, {}, current_mapping) else: current_mapping[typedef.name] = scope + typedef.name diff --git a/modules/python/generator/visp_python_bindgen/utils.py b/modules/python/generator/visp_python_bindgen/utils.py index eddc1922aa..cc4a720d3b 100644 --- a/modules/python/generator/visp_python_bindgen/utils.py +++ b/modules/python/generator/visp_python_bindgen/utils.py @@ -165,6 +165,19 @@ def get_name(name: types.PQName) -> str: ''' return '::'.join([segment.name for segment in name.segments]) +def typedef_is_anonymous(type: types.DecoratedType) -> bool: + if isinstance(type, types.Array): + return typedef_is_anonymous(type.array_of) + elif isinstance(type, types.Pointer): + return typedef_is_anonymous(type.ptr_to) + elif isinstance(type, types.MoveReference): + return typedef_is_anonymous(type.moveref_to) + elif isinstance(type, types.Reference): + return typedef_is_anonymous(type.ref_to) + elif isinstance(type, types.Type): + return name_is_anonymous(type.typename) + return False + def name_is_anonymous(name: types.PQName) -> bool: return any(isinstance(s, types.AnonymousName) for s in name.segments) @@ -245,11 +258,18 @@ def get_type(param: Union[types.FunctionType, types.DecoratedType, types.Value], else: return None elif isinstance(param, types.Pointer): - repr_str = get_type(param.ptr_to, owner_specs, header_env_mapping) - if repr_str is not None: - return repr_str + '*' + if not isinstance(param.ptr_to, types.FunctionType): + repr_str = get_type(param.ptr_to, owner_specs, header_env_mapping) + if repr_str is not None: + return repr_str + '*' + else: + return None else: - return None + fn: types.FunctionType = param.ptr_to + return_type = get_type(fn.return_type, owner_specs, header_env_mapping) + param_types = [get_type(p.type, owner_specs, header_env_mapping) for p in fn.parameters] + return f'{return_type}(*)({",".join(param_types)})' + elif isinstance(param, types.Array): repr_str = get_type(param.array_of, owner_specs, header_env_mapping) if repr_str is not None: From 657aa7bd21f9408437c7d11aecdbf858994081db Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Wed, 19 Jun 2024 14:14:00 +0200 Subject: [PATCH 10/14] Debug function pointer tripping up python bindings compilation --- .../python/generator/visp_python_bindgen/methods.py | 11 ++++++++++- modules/python/generator/visp_python_bindgen/utils.py | 10 ++++++++++ 2 files changed, 20 insertions(+), 1 deletion(-) diff --git a/modules/python/generator/visp_python_bindgen/methods.py b/modules/python/generator/visp_python_bindgen/methods.py index 913f7e9b28..92164afcba 100644 --- a/modules/python/generator/visp_python_bindgen/methods.py +++ b/modules/python/generator/visp_python_bindgen/methods.py @@ -276,6 +276,7 @@ def define_method(method: types.Method, method_config: Dict, is_class_method, sp method_name = get_name(method.name) py_method_name = method_config.get('custom_name') or method_name return_type = get_type(method.return_type, specs, header_env.mapping) + param_is_function_ptr = [is_function_pointer(param.type) for param in method.parameters] method_signature = get_method_signature(method_name, get_type(method.return_type, {}, header_env.mapping), @@ -353,7 +354,15 @@ def make_keep_alive_str(values) -> str: # Arguments that are inputs to the lambda function that wraps the ViSP function input_param_types = [params_strs[i] for i in range(len(param_is_input)) if param_is_input[i]] - params_with_names = [t + ' ' + name for t, name in zip(input_param_types, input_param_names)] + + def to_argument_name(type: str, name: str) -> str: + if '(*)' in type: + return type.replace('(*)', f'(*{name})', 1) + else: + return type + ' ' + name + + + params_with_names = [to_argument_name(t, name) for t, name in zip(input_param_types, input_param_names)] # Params that are only outputs: they should be declared in function. Assume that they are default constructible param_is_only_output = [not is_input and is_output for is_input, is_output in zip(param_is_input, param_is_output)] diff --git a/modules/python/generator/visp_python_bindgen/utils.py b/modules/python/generator/visp_python_bindgen/utils.py index cc4a720d3b..0bbd4efeb8 100644 --- a/modules/python/generator/visp_python_bindgen/utils.py +++ b/modules/python/generator/visp_python_bindgen/utils.py @@ -225,6 +225,8 @@ def segment_repr(segment: types.PQNameSegment) -> str: return '::'.join(final_segment_reprs) + + def get_type(param: Union[types.FunctionType, types.DecoratedType, types.Value], owner_specs: Dict[str, str], header_env_mapping: Dict[str, str]) -> Optional[str]: ''' Get the type of a parameter. Compared to get_typename, this function resolves the parameter's constness, whether it is a ref, moveref or pointer. @@ -350,6 +352,14 @@ def is_pointer_to_const_cstr(param: types.Pointer) -> bool: return False +def is_function_pointer(param: types.DecoratedType) -> bool: + ''' + Whether the typed is a function pointer + ''' + if not isinstance(param, types.Pointer): + return False + return isinstance(param.ptr_to, types.FunctionType) + def is_non_const_ref_to_immutable_type(param: types.DecoratedType) -> bool: ''' Returns true if the parameter is a mutable reference to an immutable type in Python. From d51d614f36de5c39eac36e8bad8442f09096011c Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Wed, 19 Jun 2024 18:56:43 +0200 Subject: [PATCH 11/14] Fix build when deprecated functions are disabled --- .../imgproc/hough-transform/tutorial-circle-hough.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/tutorial/imgproc/hough-transform/tutorial-circle-hough.cpp b/tutorial/imgproc/hough-transform/tutorial-circle-hough.cpp index e8a3200835..5b0081a491 100644 --- a/tutorial/imgproc/hough-transform/tutorial-circle-hough.cpp +++ b/tutorial/imgproc/hough-transform/tutorial-circle-hough.cpp @@ -118,7 +118,7 @@ bool run_detection(const vpImage &I_src, vpCircleHoughTransform & drawingHelpers::display(edgeMap, "Edge map", true); } return drawingHelpers::display(I_disp, "Detection results", blockingMode); - } +} int main(int argc, char **argv) { @@ -463,10 +463,10 @@ int main(int argc, char **argv) << std::endl; return EXIT_SUCCESS; } - } + } - //! [Algo params] - vpCircleHoughTransform::vpCircleHoughTransformParameters + //! [Algo params] + vpCircleHoughTransform::vpCircleHoughTransformParams algoParams(opt_gaussianKernelSize , opt_gaussianSigma , opt_sobelKernelSize @@ -540,4 +540,4 @@ int main(int argc, char **argv) } return EXIT_SUCCESS; - } +} From 1b70c39b14d7af7e3ecb4c5d46c1c99ee3a3a028 Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Wed, 19 Jun 2024 22:12:56 +0200 Subject: [PATCH 12/14] Fix issue detected by ros2 jenkins https://build.ros2.org/job/Hdev__visp__ubuntu_jammy_amd64/222/ --- modules/core/src/tools/file/basisu_miniz.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/core/src/tools/file/basisu_miniz.h b/modules/core/src/tools/file/basisu_miniz.h index a738875b00..1b3c6bac3a 100644 --- a/modules/core/src/tools/file/basisu_miniz.h +++ b/modules/core/src/tools/file/basisu_miniz.h @@ -1224,7 +1224,7 @@ tinfl_status tinfl_decompress(tinfl_decompressor *r, const mz_uint8 *pIn_buf_nex *pOut_buf_cur++ = (mz_uint8)counter; } else { - mz_uint sym2; + mz_int16 sym2; mz_uint code_len; #if TINFL_USE_64BIT_BITBUF if (num_bits < 30) { bit_buf |= (((tinfl_bit_buf_t)MZ_READ_LE32(pIn_buf_cur)) << num_bits); pIn_buf_cur += 4; num_bits += 32; } From b53e080d7c0bc9139654fa73ac43b9844f134363 Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Thu, 20 Jun 2024 11:39:54 +0200 Subject: [PATCH 13/14] Add tests as doxygen examples --- .../camera/testCameraParametersConversion.cpp | 10 +++--- modules/core/test/camera/testJsonCamera.cpp | 10 +++--- .../core/test/camera/testXmlParserCamera.cpp | 10 +++--- .../perfColorConversion.cpp | 11 ++++--- .../image-with-dataset/perfGaussianFilter.cpp | 12 ++++--- .../image-with-dataset/perfImageAddSub.cpp | 8 ++--- .../perfImageMorphology.cpp | 12 ++++--- .../image-with-dataset/perfImageResize.cpp | 12 ++++--- .../test/image-with-dataset/perfImageWarp.cpp | 12 ++++--- .../testColorConversion.cpp | 8 ++--- .../image-with-dataset/testConversion.cpp | 20 ++++++------ .../core/test/image-with-dataset/testCrop.cpp | 28 ++++++++-------- .../image-with-dataset/testCropAdvanced.cpp | 26 +++++++-------- .../image-with-dataset/testGaussianFilter.cpp | 8 ++--- .../image-with-dataset/testImageAddSub.cpp | 8 ++--- .../testImageComparison.cpp | 12 +++---- .../image-with-dataset/testImageFilter.cpp | 9 +++--- .../testImageNormalizedCorrelation.cpp | 9 +++--- .../testImageTemplateMatching.cpp | 9 +++--- .../test/image-with-dataset/testImageWarp.cpp | 6 ++-- .../test/image-with-dataset/testIoEXR.cpp | 8 ++--- .../test/image-with-dataset/testIoPFM.cpp | 8 ++--- .../test/image-with-dataset/testIoPGM.cpp | 8 ++--- .../test/image-with-dataset/testIoPPM.cpp | 8 ++--- .../image-with-dataset/testPerformanceLUT.cpp | 2 +- .../test/image-with-dataset/testReadImage.cpp | 8 ++--- .../image-with-dataset/testUndistortImage.cpp | 5 ++- modules/core/test/image/testImageBinarise.cpp | 11 ++----- .../core/test/image/testImageDifference.cpp | 11 ++----- modules/core/test/image/testImageDraw.cpp | 8 ++--- modules/core/test/image/testImageGetValue.cpp | 8 ++--- .../core/test/image/testImageMeanAndStdev.cpp | 8 ++--- .../core/test/image/testImageMorphology.cpp | 8 ++--- .../core/test/image/testImageOwnership.cpp | 8 ++--- modules/core/test/image/testImagePoint.cpp | 8 ++--- modules/core/test/image/testImagePrint.cpp | 8 ++--- modules/core/test/image/testImageResize.cpp | 8 ++--- .../test/math/perfColVectorOperations.cpp | 12 ++++--- .../test/math/perfMatrixMultiplication.cpp | 12 ++++--- .../core/test/math/perfMatrixTranspose.cpp | 12 ++++--- modules/core/test/math/testArray2D.cpp | 6 ++-- modules/core/test/math/testColVector.cpp | 8 ++--- .../core/test/math/testEigenConversion.cpp | 16 ++++------ .../core/test/math/testHomogeneousMatrix.cpp | 10 +++--- .../test/math/testJsonArrayConversion.cpp | 8 ++--- .../core/test/math/testKalmanAcceleration.cpp | 8 ++--- modules/core/test/math/testKalmanVelocity.cpp | 8 ++--- modules/core/test/math/testLineFitting.cpp | 8 ++--- modules/core/test/math/testMath.cpp | 8 ++--- modules/core/test/math/testMathUtils.cpp | 12 +++---- modules/core/test/math/testMatrix.cpp | 8 ++--- modules/core/test/math/testMatrixCholesky.cpp | 8 ++--- .../test/math/testMatrixConditionNumber.cpp | 8 ++--- .../core/test/math/testMatrixConvolution.cpp | 8 ++--- .../core/test/math/testMatrixDeterminant.cpp | 12 +++---- .../core/test/math/testMatrixException.cpp | 8 ++--- .../test/math/testMatrixInitialization.cpp | 8 ++--- modules/core/test/math/testMatrixInverse.cpp | 8 ++--- .../test/math/testMatrixPseudoInverse.cpp | 8 ++--- modules/core/test/math/testMomentAlpha.cpp | 8 ++--- modules/core/test/math/testPoseVector.cpp | 8 ++--- modules/core/test/math/testQuaternion.cpp | 3 +- modules/core/test/math/testRand.cpp | 12 ++++--- modules/core/test/math/testRobust.cpp | 8 ++--- modules/core/test/math/testRotation.cpp | 5 ++- modules/core/test/math/testRowVector.cpp | 8 ++--- modules/core/test/math/testSPC.cpp | 5 ++- modules/core/test/math/testSvd.cpp | 9 +++--- .../core/test/math/testTranslationVector.cpp | 8 ++--- modules/core/test/math/testTwistMatrix.cpp | 8 ++--- .../math/testXmlParserHomogeneousMatrix.cpp | 8 ++--- modules/core/test/munkres/testMunkres.cpp | 8 ++--- modules/core/test/network/testClient.cpp | 13 +++----- modules/core/test/network/testServer.cpp | 13 +++----- modules/core/test/network/testUDPClient.cpp | 10 +++--- modules/core/test/network/testUDPServer.cpp | 8 ++--- .../core/test/tools/convert/testConvert.cpp | 8 ++--- .../tools/cpu-features/testCPUFeatures.cpp | 11 ++++--- modules/core/test/tools/endian/testEndian.cpp | 11 ++++--- .../test/tools/geometry/testImageCircle.cpp | 7 ++-- .../core/test/tools/geometry/testPolygon.cpp | 10 +++--- .../core/test/tools/geometry/testPolygon2.cpp | 12 +++---- modules/core/test/tools/geometry/testRect.cpp | 9 +++--- .../geometry/testXmlParserRectOriented.cpp | 8 ++--- .../histogram-with-dataset/testHistogram.cpp | 11 ++----- .../tools/io-with-dataset/testIoTools.cpp | 12 +++---- .../test/tools/io/testBuildInformation.cpp | 11 ++----- modules/core/test/tools/io/testNPZ.cpp | 9 +++--- .../core/test/tools/serial/testSerialRead.cpp | 10 +++--- .../test/tools/serial/testSerialWrite.cpp | 8 ++--- modules/core/test/tools/time/testTime.cpp | 8 ++--- modules/core/test/tools/xml/testXmlParser.cpp | 2 +- .../perfApriltagDetection.cpp | 11 ++++--- .../apriltag-with-dataset/testAprilTag.cpp | 8 ++--- .../test/display-with-dataset/testClick.cpp | 16 ++++------ .../testDisplayScaled.cpp | 11 ++++--- .../display-with-dataset/testMouseEvent.cpp | 9 +++--- .../display-with-dataset/testVideoDevice.cpp | 32 ++++++++----------- .../test/display/testDisplayPolygonLines.cpp | 8 ++--- modules/gui/test/display/testDisplayRoi.cpp | 12 +++---- modules/gui/test/display/testDisplays.cpp | 29 +++++++---------- .../test/with-dataset/testAutoThreshold.cpp | 2 +- .../with-dataset/testConnectedComponents.cpp | 2 +- .../test/with-dataset/testContours.cpp | 2 +- .../test/with-dataset/testFloodFill.cpp | 2 +- .../imgproc/test/with-dataset/testImgproc.cpp | 2 +- .../image-with-dataset/perfImageLoadSave.cpp | 12 ++++--- .../image-with-dataset/testImageLoadSave.cpp | 12 ++++--- modules/io/test/testJsonArgumentParser.cpp | 8 ++--- modules/io/test/video/testVideo.cpp | 8 ++--- modules/robot/test/bebop2/testRobotBebop2.cpp | 11 ++----- .../robot/test/qbdevice/testQbSoftHand.cpp | 8 ++--- modules/robot/test/servo-afma4/testAfma4.cpp | 10 +++--- .../robot/test/servo-afma4/testRobotAfma4.cpp | 8 ++--- modules/robot/test/servo-afma6/testAfma6.cpp | 8 ++--- .../robot/test/servo-afma6/testRobotAfma6.cpp | 8 ++--- .../test/servo-afma6/testRobotAfma6Pose.cpp | 8 ++--- .../test/servo-flir-ptu/testRobotFlirPtu.cpp | 8 ++--- .../testFrankaCartForceTorque-2.cpp | 8 ++--- .../testFrankaCartForceTorque.cpp | 8 ++--- .../servo-franka/testFrankaCartVelocity-2.cpp | 10 +++--- .../servo-franka/testFrankaCartVelocity-3.cpp | 10 +++--- .../servo-franka/testFrankaCartVelocity.cpp | 10 +++--- .../test/servo-franka/testFrankaGetPose.cpp | 10 +++--- .../servo-franka/testFrankaJointPosition.cpp | 10 +++--- .../servo-franka/testFrankaJointTorque.cpp | 8 ++--- .../testFrankaJointVelocity-2.cpp | 10 +++--- .../testFrankaJointVelocity-3.cpp | 8 ++--- .../servo-franka/testFrankaJointVelocity.cpp | 10 +++--- .../testFrankaJointVelocityLimits.cpp | 8 ++--- .../testPixhawkDroneKeyboard.cpp | 8 ++--- ...estPixhawkDronePositionAbsoluteControl.cpp | 8 ++--- ...estPixhawkDronePositionRelativeControl.cpp | 8 ++--- .../servo-pixhawk/testPixhawkDroneTakeoff.cpp | 8 ++--- .../testPixhawkDroneVelocityControl.cpp | 8 ++--- .../testPixhawkRoverVelocityControl.cpp | 8 ++--- .../test/servo-pololu/testPololuPosition.cpp | 2 +- .../test/servo-pololu/testPololuVelocity.cpp | 2 +- .../testUniversalRobotsCartPosition.cpp | 8 ++--- .../testUniversalRobotsCartVelocity.cpp | 8 ++--- .../testUniversalRobotsGetData.cpp | 8 ++--- .../testUniversalRobotsJointPosition.cpp | 8 ++--- .../testUniversalRobotsJointVelocity.cpp | 8 ++--- .../servo-viper/testRobotViper650-frames.cpp | 8 ++--- .../servo-viper/testRobotViper850-frames.cpp | 8 ++--- .../test/servo-viper/testRobotViper850.cpp | 10 +++--- .../servo-viper/testRobotViper850Pose.cpp | 10 +++--- .../robot/test/servo-viper/testViper650.cpp | 8 ++--- .../robot/test/servo-viper/testViper850.cpp | 8 ++--- modules/robot/test/virtuose/testVirtuose.cpp | 10 +++--- .../robot/test/virtuose/testVirtuoseAfma6.cpp | 13 +++----- .../test/virtuose/testVirtuoseHapticBox.cpp | 12 ++----- .../test/virtuose/testVirtuoseJointLimits.cpp | 11 ++----- .../virtuose/testVirtuosePeriodicFunction.cpp | 8 ++--- .../test/virtuose/testVirtuoseWithGlove.cpp | 9 ++---- .../sensor/test/force-torque/testComedi.cpp | 3 +- .../test/force-torque/testForceTorqueAti.cpp | 10 +++--- .../testForceTorqueAtiNetFTSensor.cpp | 8 ++--- .../force-torque/testForceTorqueIitSensor.cpp | 8 ++--- .../test/framegrabber/test1394TwoGrabber.cpp | 12 +++---- .../test/framegrabber/test1394TwoResetBus.cpp | 26 ++++++--------- .../test/framegrabber/testPylonGrabber.cpp | 15 +++------ .../sensor/test/mocap/testMocapQualisys.cpp | 8 ++--- modules/sensor/test/mocap/testMocapVicon.cpp | 8 ++--- .../testOccipitalStructure_Core_images.cpp | 8 ++--- .../testOccipitalStructure_Core_imu.cpp | 8 ++--- .../testOccipitalStructure_Core_pcl.cpp | 8 ++--- .../test/rgb-depth/testRealSense2_D435.cpp | 8 ++--- .../rgb-depth/testRealSense2_D435_align.cpp | 8 ++--- .../rgb-depth/testRealSense2_D435_opencv.cpp | 8 ++--- .../rgb-depth/testRealSense2_D435_pcl.cpp | 8 ++--- .../test/rgb-depth/testRealSense2_SR300.cpp | 8 ++--- .../rgb-depth/testRealSense2_T265_images.cpp | 8 ++--- .../testRealSense2_T265_images_odometry.cpp | 8 ++--- ...tRealSense2_T265_images_odometry_async.cpp | 8 ++--- .../rgb-depth/testRealSense2_T265_imu.cpp | 8 ++--- .../testRealSense2_T265_odometry.cpp | 8 ++--- .../testRealSense2_T265_undistort.cpp | 10 +++--- .../tracking-with-dataset/testTrackDot.cpp | 8 ++--- .../perfGenericTracker.cpp | 11 ++++--- .../testGenericTracker.cpp | 12 +++---- .../testGenericTrackerCAOParsing.cpp | 12 ++++--- .../testGenericTrackerDepth.cpp | 8 ++--- .../testGenericTrackerDeterminist.cpp | 12 ++++--- .../testMbtJsonSettings.cpp | 8 ++--- .../testMbtXmlGenericParser.cpp | 12 +++---- .../tracker/mbt/test/testTukeyEstimator.cpp | 8 ++--- modules/tracker/me/test/testJsonMe.cpp | 2 +- modules/tracker/me/test/testNurbs.cpp | 2 +- .../test/homography/testDisplacement.cpp | 2 +- .../keypoint-with-dataset/testKeyPoint-2.cpp | 13 ++++---- .../keypoint-with-dataset/testKeyPoint-3.cpp | 15 +++++---- .../keypoint-with-dataset/testKeyPoint-4.cpp | 15 +++++---- .../keypoint-with-dataset/testKeyPoint-5.cpp | 15 +++++---- .../keypoint-with-dataset/testKeyPoint-6.cpp | 21 ++++++------ .../keypoint-with-dataset/testKeyPoint-7.cpp | 13 ++++---- .../keypoint-with-dataset/testKeyPoint.cpp | 13 ++++---- .../testXmlConfigParserKeyPoint.cpp | 2 +- .../pose-with-dataset/testPoseRansac2.cpp | 6 +++- modules/vision/test/pose/testFindMatch.cpp | 2 +- modules/vision/test/pose/testPose.cpp | 2 +- modules/vision/test/pose/testPoseFeatures.cpp | 2 +- modules/vision/test/pose/testPoseRansac.cpp | 4 +-- .../vs/test/visual-feature/testFeature.cpp | 9 ++---- .../test/visual-feature/testFeatureMoment.cpp | 13 +++++--- .../visual-feature/testFeatureSegment.cpp | 21 ++++++------ 206 files changed, 802 insertions(+), 1111 deletions(-) diff --git a/modules/core/test/camera/testCameraParametersConversion.cpp b/modules/core/test/camera/testCameraParametersConversion.cpp index ce807c3b93..a60814cbda 100644 --- a/modules/core/test/camera/testCameraParametersConversion.cpp +++ b/modules/core/test/camera/testCameraParametersConversion.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,11 +30,10 @@ * Description: * Performs various tests on the vpPixelMeterConversion and * vpPixelMeterConversion class. - * -*****************************************************************************/ + */ /*! - \file testCameraParametersConversion.cpp + \example testCameraParametersConversion.cpp Performs various tests on the vpPixelMeterConversion and vpPixelMeterConversion class. diff --git a/modules/core/test/camera/testJsonCamera.cpp b/modules/core/test/camera/testJsonCamera.cpp index 2eea57cf25..69f19466b1 100644 --- a/modules/core/test/camera/testJsonCamera.cpp +++ b/modules/core/test/camera/testJsonCamera.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,10 @@ * * Description: * Test vpCameraParameters JSON parse / save. - * -*****************************************************************************/ + */ /*! - \file testJsonCamera.cpp + \example testJsonCamera.cpp Test saving and parsing JSON configuration for vpCameraParameters. */ diff --git a/modules/core/test/camera/testXmlParserCamera.cpp b/modules/core/test/camera/testXmlParserCamera.cpp index 37c531b673..b10c61c393 100644 --- a/modules/core/test/camera/testXmlParserCamera.cpp +++ b/modules/core/test/camera/testXmlParserCamera.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,10 @@ * * Description: * Test vpXmlParserCamera parse / save. - * -*****************************************************************************/ + */ /*! - \file testXmlParserCamera.cpp + \example testXmlParserCamera.cpp Test vpXmlParserCamera parse / save. */ diff --git a/modules/core/test/image-with-dataset/perfColorConversion.cpp b/modules/core/test/image-with-dataset/perfColorConversion.cpp index 3dd6446ca3..a8bd0ea255 100644 --- a/modules/core/test/image-with-dataset/perfColorConversion.cpp +++ b/modules/core/test/image-with-dataset/perfColorConversion.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,9 +29,11 @@ * * Description: * Benchmark color image conversion. - * -*****************************************************************************/ + */ +/*! + \example perfColorConversion.cpp + */ #include #if defined(VISP_HAVE_CATCH2) && defined(VISP_HAVE_THREADS) diff --git a/modules/core/test/image-with-dataset/perfGaussianFilter.cpp b/modules/core/test/image-with-dataset/perfGaussianFilter.cpp index 39cd7f6603..b03fbfef39 100644 --- a/modules/core/test/image-with-dataset/perfGaussianFilter.cpp +++ b/modules/core/test/image-with-dataset/perfGaussianFilter.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,11 @@ * * Description: * Benchmark Gaussian filter. - * -*****************************************************************************/ + */ + +/*! + \example perfGaussianFilter.cpp + */ #include diff --git a/modules/core/test/image-with-dataset/perfImageAddSub.cpp b/modules/core/test/image-with-dataset/perfImageAddSub.cpp index 79372cf975..dc07323c56 100644 --- a/modules/core/test/image-with-dataset/perfImageAddSub.cpp +++ b/modules/core/test/image-with-dataset/perfImageAddSub.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test image addition / subtraction. - * -*****************************************************************************/ + */ #include diff --git a/modules/core/test/image-with-dataset/perfImageMorphology.cpp b/modules/core/test/image-with-dataset/perfImageMorphology.cpp index a592bc36a7..4d0282c147 100644 --- a/modules/core/test/image-with-dataset/perfImageMorphology.cpp +++ b/modules/core/test/image-with-dataset/perfImageMorphology.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,11 @@ * * Description: * Benchmark image morphology. - * -*****************************************************************************/ + */ + +/*! + \example perfImageMorphology.cpp + */ #include diff --git a/modules/core/test/image-with-dataset/perfImageResize.cpp b/modules/core/test/image-with-dataset/perfImageResize.cpp index 529a1cca71..3a5cff1597 100644 --- a/modules/core/test/image-with-dataset/perfImageResize.cpp +++ b/modules/core/test/image-with-dataset/perfImageResize.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,11 @@ * * Description: * Benchmark image resize. - * -*****************************************************************************/ + */ + +/*! + \example perfImageResize.cpp + */ #include diff --git a/modules/core/test/image-with-dataset/perfImageWarp.cpp b/modules/core/test/image-with-dataset/perfImageWarp.cpp index 0d747d07c2..58e370d24a 100644 --- a/modules/core/test/image-with-dataset/perfImageWarp.cpp +++ b/modules/core/test/image-with-dataset/perfImageWarp.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,11 @@ * * Description: * Benchmark image warping. - * -*****************************************************************************/ + */ + +/*! + \example perfImageWarp.cpp + */ #include diff --git a/modules/core/test/image-with-dataset/testColorConversion.cpp b/modules/core/test/image-with-dataset/testColorConversion.cpp index 136adae9ce..df6edf382d 100644 --- a/modules/core/test/image-with-dataset/testColorConversion.cpp +++ b/modules/core/test/image-with-dataset/testColorConversion.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test image conversion. - * -*****************************************************************************/ + */ /*! \example testColorConversion.cpp diff --git a/modules/core/test/image-with-dataset/testConversion.cpp b/modules/core/test/image-with-dataset/testConversion.cpp index 128dcf556d..0805138ddd 100644 --- a/modules/core/test/image-with-dataset/testConversion.cpp +++ b/modules/core/test/image-with-dataset/testConversion.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,13 @@ * * Description: * Test for image conversions. - * -*****************************************************************************/ + */ + +/*! + \example testConversion.cpp + + \brief Manipulation of image conversions. +*/ #include #include @@ -54,12 +58,6 @@ using namespace VISP_NAMESPACE_NAME; #endif -/*! - \example testConversion.cpp - - \brief Manipulation of image conversions. -*/ - // List of allowed command line options #define GETOPTARGS "cdi:o:n:h" diff --git a/modules/core/test/image-with-dataset/testCrop.cpp b/modules/core/test/image-with-dataset/testCrop.cpp index f974926635..e2f1526421 100644 --- a/modules/core/test/image-with-dataset/testCrop.cpp +++ b/modules/core/test/image-with-dataset/testCrop.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,17 @@ * * Description: * Test for sub-image extraction. - * -*****************************************************************************/ + */ + +/*! + \example testCrop.cpp + + \brief Create a sub-image from an image by cropping a rectangular area. + + Read an image from the disk, crop a rectangular area and save the + cropped image on the disk. + +*/ #include #include @@ -47,16 +55,6 @@ using namespace VISP_NAMESPACE_NAME; #endif -/*! - \example testCrop.cpp - - \brief Create a sub-image from an image by cropping a rectangular area. - - Read an image from the disk, crop a rectangular area and save the - cropped image on the disk. - -*/ - // List of allowed command line options #define GETOPTARGS "cdi:o:h" diff --git a/modules/core/test/image-with-dataset/testCropAdvanced.cpp b/modules/core/test/image-with-dataset/testCropAdvanced.cpp index ae1593d47f..e2349d9fe8 100644 --- a/modules/core/test/image-with-dataset/testCropAdvanced.cpp +++ b/modules/core/test/image-with-dataset/testCropAdvanced.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,17 +29,7 @@ * * Description: * Test for sub-image extraction. - * -*****************************************************************************/ - -#include -#include -#include -#include - -#ifdef ENABLE_VISP_NAMESPACE -using namespace VISP_NAMESPACE_NAME; -#endif + */ /*! \example testCropAdvanced.cpp @@ -51,6 +40,15 @@ using namespace VISP_NAMESPACE_NAME; */ +#include +#include +#include +#include + +#ifdef ENABLE_VISP_NAMESPACE +using namespace VISP_NAMESPACE_NAME; +#endif + // List of allowed command line options #define GETOPTARGS "cdi:o:h" diff --git a/modules/core/test/image-with-dataset/testGaussianFilter.cpp b/modules/core/test/image-with-dataset/testGaussianFilter.cpp index 8051517ad0..049ff45147 100644 --- a/modules/core/test/image-with-dataset/testGaussianFilter.cpp +++ b/modules/core/test/image-with-dataset/testGaussianFilter.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test Gaussian filter. - * -*****************************************************************************/ + */ /*! \example testGaussianFilter.cpp diff --git a/modules/core/test/image-with-dataset/testImageAddSub.cpp b/modules/core/test/image-with-dataset/testImageAddSub.cpp index 7aacf3a18e..aba582ba54 100644 --- a/modules/core/test/image-with-dataset/testImageAddSub.cpp +++ b/modules/core/test/image-with-dataset/testImageAddSub.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test image addition / subtraction. - * -*****************************************************************************/ + */ #include diff --git a/modules/core/test/image-with-dataset/testImageComparison.cpp b/modules/core/test/image-with-dataset/testImageComparison.cpp index 35510f384d..a1abb436be 100644 --- a/modules/core/test/image-with-dataset/testImageComparison.cpp +++ b/modules/core/test/image-with-dataset/testImageComparison.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,8 @@ * * Description: * Test the comparison of two vpImage objects of the same type. - * - * Authors: - * Souriya Trinh - * -*****************************************************************************/ + */ + /*! \example testImageComparison.cpp diff --git a/modules/core/test/image-with-dataset/testImageFilter.cpp b/modules/core/test/image-with-dataset/testImageFilter.cpp index 07e5978d85..7ff6088d91 100644 --- a/modules/core/test/image-with-dataset/testImageFilter.cpp +++ b/modules/core/test/image-with-dataset/testImageFilter.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,8 @@ * * Description: * Test some functions from vpImageFilter class. - * -*****************************************************************************/ + */ + /*! \example testImageFilter.cpp diff --git a/modules/core/test/image-with-dataset/testImageNormalizedCorrelation.cpp b/modules/core/test/image-with-dataset/testImageNormalizedCorrelation.cpp index b54c03c308..4c54702b27 100644 --- a/modules/core/test/image-with-dataset/testImageNormalizedCorrelation.cpp +++ b/modules/core/test/image-with-dataset/testImageNormalizedCorrelation.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,8 @@ * * Description: * Test vpImageTools::normalizedCorrelation(). - * -*****************************************************************************/ + */ + /*! \example testImageNormalizedCorrelation.cpp diff --git a/modules/core/test/image-with-dataset/testImageTemplateMatching.cpp b/modules/core/test/image-with-dataset/testImageTemplateMatching.cpp index 16bdc269de..24f3a6794d 100644 --- a/modules/core/test/image-with-dataset/testImageTemplateMatching.cpp +++ b/modules/core/test/image-with-dataset/testImageTemplateMatching.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,8 @@ * * Description: * Test vpImageTools::templateMatching(). - * -*****************************************************************************/ + */ + /*! \example testImageTemplateMatching.cpp diff --git a/modules/core/test/image-with-dataset/testImageWarp.cpp b/modules/core/test/image-with-dataset/testImageWarp.cpp index 136a922014..214f69638d 100644 --- a/modules/core/test/image-with-dataset/testImageWarp.cpp +++ b/modules/core/test/image-with-dataset/testImageWarp.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,8 +29,7 @@ * * Description: * Test image warping. - * -*****************************************************************************/ + */ /*! \example testImageWarp.cpp diff --git a/modules/core/test/image-with-dataset/testIoEXR.cpp b/modules/core/test/image-with-dataset/testIoEXR.cpp index 8474ca7597..a2bb27d299 100644 --- a/modules/core/test/image-with-dataset/testIoEXR.cpp +++ b/modules/core/test/image-with-dataset/testIoEXR.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test image I/O for EXR file format. - * -*****************************************************************************/ + */ /*! \example testIoEXR.cpp diff --git a/modules/core/test/image-with-dataset/testIoPFM.cpp b/modules/core/test/image-with-dataset/testIoPFM.cpp index 5dfb0d9e52..f8d253c38f 100644 --- a/modules/core/test/image-with-dataset/testIoPFM.cpp +++ b/modules/core/test/image-with-dataset/testIoPFM.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test image I/O for PFM file format. - * -*****************************************************************************/ + */ /*! \example testIoPFM.cpp diff --git a/modules/core/test/image-with-dataset/testIoPGM.cpp b/modules/core/test/image-with-dataset/testIoPGM.cpp index b5ffe32f66..20ec119e58 100644 --- a/modules/core/test/image-with-dataset/testIoPGM.cpp +++ b/modules/core/test/image-with-dataset/testIoPGM.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Read and write PGM images on the disk. - * -*****************************************************************************/ + */ #include #include diff --git a/modules/core/test/image-with-dataset/testIoPPM.cpp b/modules/core/test/image-with-dataset/testIoPPM.cpp index 783de2289f..5c74740336 100644 --- a/modules/core/test/image-with-dataset/testIoPPM.cpp +++ b/modules/core/test/image-with-dataset/testIoPPM.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Read and write PGM images on the disk. - * -*****************************************************************************/ + */ #include #include diff --git a/modules/core/test/image-with-dataset/testPerformanceLUT.cpp b/modules/core/test/image-with-dataset/testPerformanceLUT.cpp index 8cbdf167ed..ccce7dae6d 100644 --- a/modules/core/test/image-with-dataset/testPerformanceLUT.cpp +++ b/modules/core/test/image-with-dataset/testPerformanceLUT.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/core/test/image-with-dataset/testReadImage.cpp b/modules/core/test/image-with-dataset/testReadImage.cpp index 05e2a23c14..17ae5f15d4 100644 --- a/modules/core/test/image-with-dataset/testReadImage.cpp +++ b/modules/core/test/image-with-dataset/testReadImage.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Read images on the disk. - * -*****************************************************************************/ + */ #include #include diff --git a/modules/core/test/image-with-dataset/testUndistortImage.cpp b/modules/core/test/image-with-dataset/testUndistortImage.cpp index 0eb164b3b1..9b15b7b103 100644 --- a/modules/core/test/image-with-dataset/testUndistortImage.cpp +++ b/modules/core/test/image-with-dataset/testUndistortImage.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/core/test/image/testImageBinarise.cpp b/modules/core/test/image/testImageBinarise.cpp index 3dbcdba534..ab1eeb15d0 100644 --- a/modules/core/test/image/testImageBinarise.cpp +++ b/modules/core/test/image/testImageBinarise.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,7 @@ * * Description: * Test for vpImageTools::binarise() function. - * - * Authors: - * Souriya Trinh - * -*****************************************************************************/ + */ /*! \example testImageBinarise.cpp diff --git a/modules/core/test/image/testImageDifference.cpp b/modules/core/test/image/testImageDifference.cpp index 71450593e4..ea3723be71 100644 --- a/modules/core/test/image/testImageDifference.cpp +++ b/modules/core/test/image/testImageDifference.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,7 @@ * * Description: * Image difference. - * - * Authors: - * Souriya Trinh - * -*****************************************************************************/ + */ #include #include diff --git a/modules/core/test/image/testImageDraw.cpp b/modules/core/test/image/testImageDraw.cpp index 25aee19d53..39a3588c1b 100644 --- a/modules/core/test/image/testImageDraw.cpp +++ b/modules/core/test/image/testImageDraw.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test for vpImageDraw class. - * -*****************************************************************************/ + */ /*! \example testImageDraw.cpp diff --git a/modules/core/test/image/testImageGetValue.cpp b/modules/core/test/image/testImageGetValue.cpp index fe20646340..99b1c2589a 100644 --- a/modules/core/test/image/testImageGetValue.cpp +++ b/modules/core/test/image/testImageGetValue.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test for vpImagePoint::getValue(). - * -*****************************************************************************/ + */ /*! \example testImageGetValue.cpp diff --git a/modules/core/test/image/testImageMeanAndStdev.cpp b/modules/core/test/image/testImageMeanAndStdev.cpp index c5280e7b81..c840390a74 100644 --- a/modules/core/test/image/testImageMeanAndStdev.cpp +++ b/modules/core/test/image/testImageMeanAndStdev.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test for vpImagePoint::getValue(). - * -*****************************************************************************/ + */ /*! \example testImageMeanAndStdev.cpp diff --git a/modules/core/test/image/testImageMorphology.cpp b/modules/core/test/image/testImageMorphology.cpp index 2125ffbfaa..d21ffe427f 100644 --- a/modules/core/test/image/testImageMorphology.cpp +++ b/modules/core/test/image/testImageMorphology.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test image morphology. - * -*****************************************************************************/ + */ /*! \example testImageMorphology.cpp diff --git a/modules/core/test/image/testImageOwnership.cpp b/modules/core/test/image/testImageOwnership.cpp index 72af85a704..516284ec92 100644 --- a/modules/core/test/image/testImageOwnership.cpp +++ b/modules/core/test/image/testImageOwnership.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test vpImage ownership - * -*****************************************************************************/ + */ #include diff --git a/modules/core/test/image/testImagePoint.cpp b/modules/core/test/image/testImagePoint.cpp index c226befd43..62578f01c4 100644 --- a/modules/core/test/image/testImagePoint.cpp +++ b/modules/core/test/image/testImagePoint.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test for vpImagePoint class. - * -*****************************************************************************/ + */ /*! \example testImagePoint.cpp diff --git a/modules/core/test/image/testImagePrint.cpp b/modules/core/test/image/testImagePrint.cpp index 9144ae019a..8993259655 100644 --- a/modules/core/test/image/testImagePrint.cpp +++ b/modules/core/test/image/testImagePrint.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test image print. - * -*****************************************************************************/ + */ #include #include diff --git a/modules/core/test/image/testImageResize.cpp b/modules/core/test/image/testImageResize.cpp index 6e77322767..d5afc27af4 100644 --- a/modules/core/test/image/testImageResize.cpp +++ b/modules/core/test/image/testImageResize.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test image resize. - * -*****************************************************************************/ + */ /*! \example testImageResize.cpp diff --git a/modules/core/test/math/perfColVectorOperations.cpp b/modules/core/test/math/perfColVectorOperations.cpp index 344f739c0e..42d239e6c4 100644 --- a/modules/core/test/math/perfColVectorOperations.cpp +++ b/modules/core/test/math/perfColVectorOperations.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,11 @@ * * Description: * Benchmark column vector operations. - * -*****************************************************************************/ + */ + +/*! + \example perfColVectorOperations.cpp + */ #include diff --git a/modules/core/test/math/perfMatrixMultiplication.cpp b/modules/core/test/math/perfMatrixMultiplication.cpp index 0e200134c6..30e40d8e28 100644 --- a/modules/core/test/math/perfMatrixMultiplication.cpp +++ b/modules/core/test/math/perfMatrixMultiplication.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,11 @@ * * Description: * Benchmark matrix multiplication. - * -*****************************************************************************/ + */ + +/*! + \example perfMatrixMultiplication.cpp + */ #include diff --git a/modules/core/test/math/perfMatrixTranspose.cpp b/modules/core/test/math/perfMatrixTranspose.cpp index d2a50d5e30..74a7128493 100644 --- a/modules/core/test/math/perfMatrixTranspose.cpp +++ b/modules/core/test/math/perfMatrixTranspose.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,11 @@ * * Description: * Benchmark matrix transpose. - * -*****************************************************************************/ + */ + +/*! + \example perfMatrixTranspose.cpp + */ #include diff --git a/modules/core/test/math/testArray2D.cpp b/modules/core/test/math/testArray2D.cpp index 9194e0f2ff..eeab0ece1a 100644 --- a/modules/core/test/math/testArray2D.cpp +++ b/modules/core/test/math/testArray2D.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2024 by Inria. All rights reserved. * @@ -30,8 +29,7 @@ * * Description: * Test some vpColVector functionalities. - * -*****************************************************************************/ + */ /*! \example testArray2D.cpp diff --git a/modules/core/test/math/testColVector.cpp b/modules/core/test/math/testColVector.cpp index 94f2654557..805d86ec57 100644 --- a/modules/core/test/math/testColVector.cpp +++ b/modules/core/test/math/testColVector.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test some vpColVector functionalities. - * -*****************************************************************************/ + */ /*! \example testColVector.cpp diff --git a/modules/core/test/math/testEigenConversion.cpp b/modules/core/test/math/testEigenConversion.cpp index 4bd11c6232..10fb1b1e31 100644 --- a/modules/core/test/math/testEigenConversion.cpp +++ b/modules/core/test/math/testEigenConversion.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test conversion between ViSP and Eigen type. - * -*****************************************************************************/ + */ /*! \example testEigenConversion.cpp @@ -125,7 +123,7 @@ TEST_CASE("Eigen::MatrixXd <--> vpMatrix conversion", "[eigen_conversion]") VISP_NAMESPACE_NAME::eigen2visp(eigen_m2, visp_m2); REQUIRE(visp_m == visp_m2); std::cout << std::endl; -} + } TEST_CASE("Eigen::MatrixX4d <--> vpMatrix conversion", "[eigen_conversion]") { @@ -154,7 +152,7 @@ TEST_CASE("Eigen::MatrixX4d <--> vpMatrix conversion", "[eigen_conversion]") VISP_NAMESPACE_NAME::eigen2visp(eigen_m2, visp_m2); REQUIRE(visp_m == visp_m2); std::cout << std::endl; -} + } TEST_CASE("Eigen::Matrix <--> vpMatrix conversion", "[eigen_conversion]") { @@ -183,7 +181,7 @@ TEST_CASE("Eigen::Matrix <--> vpMatrix conve VISP_NAMESPACE_NAME::eigen2visp(eigen_m2, visp_m2); REQUIRE(visp_m == visp_m2); std::cout << std::endl; -} + } TEST_CASE("Eigen::Matrix <--> vpMatrix conversion", "[eigen_conversion]") { @@ -212,7 +210,7 @@ TEST_CASE("Eigen::Matrix <--> vpMatrix conve VISP_NAMESPACE_NAME::eigen2visp(eigen_m2, visp_m2); REQUIRE(visp_m == visp_m2); std::cout << std::endl; -} + } TEST_CASE("vpHomogeneousMatrix <--> Eigen::Matrix4d conversion", "[eigen_conversion]") { diff --git a/modules/core/test/math/testHomogeneousMatrix.cpp b/modules/core/test/math/testHomogeneousMatrix.cpp index 9a3c7549a0..6c77bb83d4 100644 --- a/modules/core/test/math/testHomogeneousMatrix.cpp +++ b/modules/core/test/math/testHomogeneousMatrix.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,10 +29,11 @@ * * Description: * Test some vpHomogeneousMatrix functionalities. - * -*****************************************************************************/ + */ /*! + \example testHomogeneousMatrix.cpp + Test some vpHomogeneousMatrix functionalities. */ #include diff --git a/modules/core/test/math/testJsonArrayConversion.cpp b/modules/core/test/math/testJsonArrayConversion.cpp index a2a88dd73d..3bd5d00df8 100644 --- a/modules/core/test/math/testJsonArrayConversion.cpp +++ b/modules/core/test/math/testJsonArrayConversion.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test vpArray2D and children JSON parse / save. - * -*****************************************************************************/ + */ /*! \file testJsonArrayConversion.cpp diff --git a/modules/core/test/math/testKalmanAcceleration.cpp b/modules/core/test/math/testKalmanAcceleration.cpp index 32b2ce93da..92ce19029d 100644 --- a/modules/core/test/math/testKalmanAcceleration.cpp +++ b/modules/core/test/math/testKalmanAcceleration.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Tests some vpLinearKalmanFilterInstantiation functionalities. - * -*****************************************************************************/ + */ /*! \example testKalmanAcceleration.cpp diff --git a/modules/core/test/math/testKalmanVelocity.cpp b/modules/core/test/math/testKalmanVelocity.cpp index cef84f56ac..bc69d6ff46 100644 --- a/modules/core/test/math/testKalmanVelocity.cpp +++ b/modules/core/test/math/testKalmanVelocity.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Tests some vpLinearKalmanFilterInstantiation functionalities. - * -*****************************************************************************/ + */ /*! \example testKalmanVelocity.cpp diff --git a/modules/core/test/math/testLineFitting.cpp b/modules/core/test/math/testLineFitting.cpp index b5e33fd947..a626448bc3 100644 --- a/modules/core/test/math/testLineFitting.cpp +++ b/modules/core/test/math/testLineFitting.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test line fitting. - * -*****************************************************************************/ + */ /*! \example testLineFitting.cpp diff --git a/modules/core/test/math/testMath.cpp b/modules/core/test/math/testMath.cpp index 7b45907617..23292c7aca 100644 --- a/modules/core/test/math/testMath.cpp +++ b/modules/core/test/math/testMath.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test some core functionalities. - * -*****************************************************************************/ + */ /*! \example testMath.cpp diff --git a/modules/core/test/math/testMathUtils.cpp b/modules/core/test/math/testMathUtils.cpp index 3a55d02afe..d01922304c 100644 --- a/modules/core/test/math/testMathUtils.cpp +++ b/modules/core/test/math/testMathUtils.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test additional math functions such as lon-lat generator or look-at function. - * -*****************************************************************************/ + */ /*! \example testMathUtils.cpp @@ -136,11 +134,11 @@ TEST_CASE("Lon-Lat generator", "[math_lonlat]") if (file.is_open()) { (ecef_M_enu * enu_M_cv).save(file); } - } + } #endif + } } } -} TEST_CASE("Equidistributed sphere point", "[math_equi_sphere_pts]") { diff --git a/modules/core/test/math/testMatrix.cpp b/modules/core/test/math/testMatrix.cpp index b4200852fe..7092d0aa33 100644 --- a/modules/core/test/math/testMatrix.cpp +++ b/modules/core/test/math/testMatrix.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test some vpMatrix functionalities. - * -*****************************************************************************/ + */ /*! \example testMatrix.cpp diff --git a/modules/core/test/math/testMatrixCholesky.cpp b/modules/core/test/math/testMatrixCholesky.cpp index 24db7d6dba..0e693b90f9 100644 --- a/modules/core/test/math/testMatrixCholesky.cpp +++ b/modules/core/test/math/testMatrixCholesky.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test matrix condition number. - * -*****************************************************************************/ + */ /*! \example testMatrixCholesky.cpp diff --git a/modules/core/test/math/testMatrixConditionNumber.cpp b/modules/core/test/math/testMatrixConditionNumber.cpp index 54363c578a..daabb07e13 100644 --- a/modules/core/test/math/testMatrixConditionNumber.cpp +++ b/modules/core/test/math/testMatrixConditionNumber.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test matrix condition number. - * -*****************************************************************************/ + */ /*! \example testMatrixConditionNumber.cpp diff --git a/modules/core/test/math/testMatrixConvolution.cpp b/modules/core/test/math/testMatrixConvolution.cpp index 02f59cd9f8..868abd1d92 100644 --- a/modules/core/test/math/testMatrixConvolution.cpp +++ b/modules/core/test/math/testMatrixConvolution.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test matrix convolution. - * -*****************************************************************************/ + */ /*! \example testMatrixConvolution.cpp diff --git a/modules/core/test/math/testMatrixDeterminant.cpp b/modules/core/test/math/testMatrixDeterminant.cpp index 0b6a0d4d2d..b093232775 100644 --- a/modules/core/test/math/testMatrixDeterminant.cpp +++ b/modules/core/test/math/testMatrixDeterminant.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test various determinant computation methods. - * -*****************************************************************************/ + */ /*! \example testMatrixDeterminant.cpp @@ -399,9 +397,9 @@ int main(int argc, const char *argv[]) std::cout << "Test does nothing since you dont't have Lapack, Eigen3 or OpenCV 3rd party" << std::endl; return EXIT_SUCCESS; #endif - } + } catch (const vpException &e) { std::cout << "Catch an exception: " << e.getStringMessage() << std::endl; return EXIT_FAILURE; } - } +} diff --git a/modules/core/test/math/testMatrixException.cpp b/modules/core/test/math/testMatrixException.cpp index 5da0509f34..d7063345c2 100644 --- a/modules/core/test/math/testMatrixException.cpp +++ b/modules/core/test/math/testMatrixException.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test matrix exceptions. - * -*****************************************************************************/ + */ /*! \example testMatrixException.cpp diff --git a/modules/core/test/math/testMatrixInitialization.cpp b/modules/core/test/math/testMatrixInitialization.cpp index 11127a34a0..5a3cc2ce7e 100644 --- a/modules/core/test/math/testMatrixInitialization.cpp +++ b/modules/core/test/math/testMatrixInitialization.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test Matrix initialization. - * -*****************************************************************************/ + */ /*! \example testMatrixInitialization.cpp diff --git a/modules/core/test/math/testMatrixInverse.cpp b/modules/core/test/math/testMatrixInverse.cpp index 509ddd1bc3..8479e0d0c9 100644 --- a/modules/core/test/math/testMatrixInverse.cpp +++ b/modules/core/test/math/testMatrixInverse.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test various inversions. - * -*****************************************************************************/ + */ /*! \example testMatrixInverse.cpp diff --git a/modules/core/test/math/testMatrixPseudoInverse.cpp b/modules/core/test/math/testMatrixPseudoInverse.cpp index ec5f195b56..baec834498 100644 --- a/modules/core/test/math/testMatrixPseudoInverse.cpp +++ b/modules/core/test/math/testMatrixPseudoInverse.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test various svd decompositions. - * -*****************************************************************************/ + */ /*! \example testMatrixPseudoInverse.cpp diff --git a/modules/core/test/math/testMomentAlpha.cpp b/modules/core/test/math/testMomentAlpha.cpp index 5123e070ae..40890762f3 100644 --- a/modules/core/test/math/testMomentAlpha.cpp +++ b/modules/core/test/math/testMomentAlpha.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test some vpMomentAlpha functionalities. - * -*****************************************************************************/ + */ /*! \example testMomentAlpha.cpp diff --git a/modules/core/test/math/testPoseVector.cpp b/modules/core/test/math/testPoseVector.cpp index 68602ebd22..0063fcd7a1 100644 --- a/modules/core/test/math/testPoseVector.cpp +++ b/modules/core/test/math/testPoseVector.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test some vpColVector functionalities. - * -*****************************************************************************/ + */ /*! \example testPoseVector.cpp diff --git a/modules/core/test/math/testQuaternion.cpp b/modules/core/test/math/testQuaternion.cpp index 347fc32c92..5a675b3b34 100644 --- a/modules/core/test/math/testQuaternion.cpp +++ b/modules/core/test/math/testQuaternion.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2024 by Inria. All rights reserved. * diff --git a/modules/core/test/math/testRand.cpp b/modules/core/test/math/testRand.cpp index 9943689b7e..aa07d1378f 100644 --- a/modules/core/test/math/testRand.cpp +++ b/modules/core/test/math/testRand.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,11 @@ * * Description: * Test pseudo random number generator. - * -*****************************************************************************/ + */ + +/*! + \example testRand.cpp + */ #include diff --git a/modules/core/test/math/testRobust.cpp b/modules/core/test/math/testRobust.cpp index e043f8e442..a6b3af8c79 100644 --- a/modules/core/test/math/testRobust.cpp +++ b/modules/core/test/math/testRobust.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test some vpMath functionalities. - * -*****************************************************************************/ + */ /*! \example testRobust.cpp diff --git a/modules/core/test/math/testRotation.cpp b/modules/core/test/math/testRotation.cpp index 58665da2ce..bbc5c57cb3 100644 --- a/modules/core/test/math/testRotation.cpp +++ b/modules/core/test/math/testRotation.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/core/test/math/testRowVector.cpp b/modules/core/test/math/testRowVector.cpp index 55fa99fa92..e0bf078c18 100644 --- a/modules/core/test/math/testRowVector.cpp +++ b/modules/core/test/math/testRowVector.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test some vpColVector functionalities. - * -*****************************************************************************/ + */ /*! \example testRowVector.cpp diff --git a/modules/core/test/math/testSPC.cpp b/modules/core/test/math/testSPC.cpp index 9ac1f22f10..b151517ad6 100644 --- a/modules/core/test/math/testSPC.cpp +++ b/modules/core/test/math/testSPC.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/core/test/math/testSvd.cpp b/modules/core/test/math/testSvd.cpp index 1806a12691..24b3752201 100644 --- a/modules/core/test/math/testSvd.cpp +++ b/modules/core/test/math/testSvd.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -545,9 +544,9 @@ int main(int argc, const char *argv[]) std::cout << "Test does nothing since you dont't have Lapack, Eigen3 or OpenCV 3rd party" << std::endl; return EXIT_SUCCESS; #endif - } + } catch (const vpException &e) { std::cout << "Catch an exception: " << e.getStringMessage() << std::endl; return EXIT_FAILURE; } - } +} diff --git a/modules/core/test/math/testTranslationVector.cpp b/modules/core/test/math/testTranslationVector.cpp index 5a17acb31d..7fd9aee6a5 100644 --- a/modules/core/test/math/testTranslationVector.cpp +++ b/modules/core/test/math/testTranslationVector.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test some vpColVector functionalities. - * -*****************************************************************************/ + */ /*! \example testTranslationVector.cpp diff --git a/modules/core/test/math/testTwistMatrix.cpp b/modules/core/test/math/testTwistMatrix.cpp index 105ccd356a..fd8e7b84a2 100644 --- a/modules/core/test/math/testTwistMatrix.cpp +++ b/modules/core/test/math/testTwistMatrix.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Tests some vpMatrix functionalities. - * -*****************************************************************************/ + */ /*! \example testTwistMatrix.cpp diff --git a/modules/core/test/math/testXmlParserHomogeneousMatrix.cpp b/modules/core/test/math/testXmlParserHomogeneousMatrix.cpp index d273d997b9..95aaf2beaa 100644 --- a/modules/core/test/math/testXmlParserHomogeneousMatrix.cpp +++ b/modules/core/test/math/testXmlParserHomogeneousMatrix.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test vpXmlParserHomogeneousMatrix parse / save. - * -*****************************************************************************/ + */ /*! \file testXmlParserHomogeneousMatrix.cpp diff --git a/modules/core/test/munkres/testMunkres.cpp b/modules/core/test/munkres/testMunkres.cpp index af5e2ac723..adacfe4995 100644 --- a/modules/core/test/munkres/testMunkres.cpp +++ b/modules/core/test/munkres/testMunkres.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,8 +29,7 @@ * * Description: * Test Munkres assignment algorithm. - * -*****************************************************************************/ + */ /*! \example testMunkres.cpp @@ -235,7 +233,7 @@ int main() if (not testHorMat()) { return EXIT_FAILURE; -} + } return EXIT_SUCCESS; } diff --git a/modules/core/test/network/testClient.cpp b/modules/core/test/network/testClient.cpp index 67cb120c23..b9b829fa26 100644 --- a/modules/core/test/network/testClient.cpp +++ b/modules/core/test/network/testClient.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,7 @@ * * Description: * Test for TCP client/Server. - * - * Authors: - * Aurelien Yol - * -*****************************************************************************/ + */ /*! \example testClient.cpp @@ -83,4 +78,4 @@ int main() std::cout << "This test doesn't work on win XP where inet_ntop() is not available" << std::endl; return EXIT_SUCCESS; #endif - } +} diff --git a/modules/core/test/network/testServer.cpp b/modules/core/test/network/testServer.cpp index e165e3f601..33f9974aaa 100644 --- a/modules/core/test/network/testServer.cpp +++ b/modules/core/test/network/testServer.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,7 @@ * * Description: * Test for TCP client/Server. - * - * Authors: - * Aurelien Yol - * -*****************************************************************************/ + */ /*! \example testServer.cpp @@ -85,4 +80,4 @@ int main() std::cout << "This test doesn't work on win XP where inet_ntop() is not available" << std::endl; return EXIT_SUCCESS; #endif - } +} diff --git a/modules/core/test/network/testUDPClient.cpp b/modules/core/test/network/testUDPClient.cpp index ca0b9ba582..cfb5ed5383 100644 --- a/modules/core/test/network/testUDPClient.cpp +++ b/modules/core/test/network/testUDPClient.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test for UDP client. - * -*****************************************************************************/ + */ /*! \example testUDPClient.cpp @@ -120,4 +118,4 @@ int main(int argc, char **argv) (void)argv; return EXIT_SUCCESS; #endif - } +} diff --git a/modules/core/test/network/testUDPServer.cpp b/modules/core/test/network/testUDPServer.cpp index 21d1e56d36..4373c70abd 100644 --- a/modules/core/test/network/testUDPServer.cpp +++ b/modules/core/test/network/testUDPServer.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,8 +29,7 @@ * * Description: * Test for UDP server. - * -*****************************************************************************/ + */ /*! \example testUDPServer.cpp @@ -127,4 +125,4 @@ int main() std::cout << "This test doesn't work on win XP where inet_ntop() is not available" << std::endl; return EXIT_SUCCESS; #endif - } +} diff --git a/modules/core/test/tools/convert/testConvert.cpp b/modules/core/test/tools/convert/testConvert.cpp index eea81ef634..3ce05a35a6 100644 --- a/modules/core/test/tools/convert/testConvert.cpp +++ b/modules/core/test/tools/convert/testConvert.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test functions in vpIoTools. - * -*****************************************************************************/ + */ /*! diff --git a/modules/core/test/tools/cpu-features/testCPUFeatures.cpp b/modules/core/test/tools/cpu-features/testCPUFeatures.cpp index 94e973e1b0..c5b9b5f5a4 100644 --- a/modules/core/test/tools/cpu-features/testCPUFeatures.cpp +++ b/modules/core/test/tools/cpu-features/testCPUFeatures.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,9 +29,11 @@ * * Description: * Print CPU features. - * -*****************************************************************************/ + */ +/*! + \example testCPUFeatures.cpp + */ #include #include #include diff --git a/modules/core/test/tools/endian/testEndian.cpp b/modules/core/test/tools/endian/testEndian.cpp index 100e9c15e6..eee40ac0a2 100644 --- a/modules/core/test/tools/endian/testEndian.cpp +++ b/modules/core/test/tools/endian/testEndian.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,9 +29,11 @@ * * Description: * Test vpEndian. - * -*****************************************************************************/ + */ +/*! + \example testEndian.cpp + */ #include #include #include diff --git a/modules/core/test/tools/geometry/testImageCircle.cpp b/modules/core/test/tools/geometry/testImageCircle.cpp index 9900e0e78e..e2eda4b8fd 100644 --- a/modules/core/test/tools/geometry/testImageCircle.cpp +++ b/modules/core/test/tools/geometry/testImageCircle.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -28,9 +28,12 @@ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. * * Description: - * Test vpRect and vpImagePoint. + * Test vpImageCircle. */ +/*! + \example testImageCircle.cpp + */ #include #include #include diff --git a/modules/core/test/tools/geometry/testPolygon.cpp b/modules/core/test/tools/geometry/testPolygon.cpp index 000cece60b..ecefcab6a1 100644 --- a/modules/core/test/tools/geometry/testPolygon.cpp +++ b/modules/core/test/tools/geometry/testPolygon.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,8 +29,11 @@ * * Description: * Example which test the polygon. - * -*****************************************************************************/ + */ + +/*! + \example testPolygon.cpp + */ #include #include #include diff --git a/modules/core/test/tools/geometry/testPolygon2.cpp b/modules/core/test/tools/geometry/testPolygon2.cpp index 97577f8e7f..e20aef66a0 100644 --- a/modules/core/test/tools/geometry/testPolygon2.cpp +++ b/modules/core/test/tools/geometry/testPolygon2.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,8 @@ * * Description: * Test vpPolygon class. - * - * Authors: - * Julien Dufour - * -*****************************************************************************/ + */ + /*! \example testPolygon2.cpp diff --git a/modules/core/test/tools/geometry/testRect.cpp b/modules/core/test/tools/geometry/testRect.cpp index 1c782f3832..77a6ff6cd2 100644 --- a/modules/core/test/tools/geometry/testRect.cpp +++ b/modules/core/test/tools/geometry/testRect.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,9 +29,11 @@ * * Description: * Test vpRect. - * -*****************************************************************************/ + */ +/*! + \example testRect.cpp + */ #include #include #include diff --git a/modules/core/test/tools/geometry/testXmlParserRectOriented.cpp b/modules/core/test/tools/geometry/testXmlParserRectOriented.cpp index e25f8bc8e1..4d2dfce34d 100644 --- a/modules/core/test/tools/geometry/testXmlParserRectOriented.cpp +++ b/modules/core/test/tools/geometry/testXmlParserRectOriented.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test vpXmlParserRectOriented parse / save. - * -*****************************************************************************/ + */ /*! \file testXmlParserRectOriented.cpp diff --git a/modules/core/test/tools/histogram-with-dataset/testHistogram.cpp b/modules/core/test/tools/histogram-with-dataset/testHistogram.cpp index c04bbb799f..e023251cc8 100644 --- a/modules/core/test/tools/histogram-with-dataset/testHistogram.cpp +++ b/modules/core/test/tools/histogram-with-dataset/testHistogram.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,7 @@ * * Description: * Test histogram computation. - * - * Authors: - * Souriya Trinh - * -*****************************************************************************/ + */ #include #include diff --git a/modules/core/test/tools/io-with-dataset/testIoTools.cpp b/modules/core/test/tools/io-with-dataset/testIoTools.cpp index bc46bf9874..05828442cb 100644 --- a/modules/core/test/tools/io-with-dataset/testIoTools.cpp +++ b/modules/core/test/tools/io-with-dataset/testIoTools.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test functions in vpIoTools. - * -*****************************************************************************/ + */ /*! \example testIoTools.cpp @@ -287,7 +285,7 @@ int main(int argc, const char **argv) if (nbFail) { std::cerr << "Failed test: vpIoTools::splitDrive (Win32)" << std::endl; return EXIT_FAILURE; -} + } #endif // Test vpIoTools::getFileExtension @@ -855,7 +853,7 @@ int main(int argc, const char **argv) if (nbFail) { std::cerr << "Failed test: vpIoTools::toLowerCase (WIN32 platform)" << std::endl; return EXIT_FAILURE; - } + } #else if (strcmp(vpIoTools::toLowerCase(testString).c_str(), expectedLower.c_str()) == 0) { nbOk++; diff --git a/modules/core/test/tools/io/testBuildInformation.cpp b/modules/core/test/tools/io/testBuildInformation.cpp index 37d8dab20c..05fa5d0c42 100644 --- a/modules/core/test/tools/io/testBuildInformation.cpp +++ b/modules/core/test/tools/io/testBuildInformation.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,7 @@ * * Description: * Test functions in vpIoTools. - * - * Authors: - * Souriya Trinh - * -*****************************************************************************/ + */ /*! \example testBuildInformation.cpp diff --git a/modules/core/test/tools/io/testNPZ.cpp b/modules/core/test/tools/io/testNPZ.cpp index c6be783b73..c321910fff 100644 --- a/modules/core/test/tools/io/testNPZ.cpp +++ b/modules/core/test/tools/io/testNPZ.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2024 by Inria. All rights reserved. * @@ -30,9 +29,11 @@ * * Description: * Test visp::cnpy::npz_load() / visp::cnpy::npy_save() functions. - * -*****************************************************************************/ + */ +/*! + \example testNPZ.cpp + */ #include #include #include diff --git a/modules/core/test/tools/serial/testSerialRead.cpp b/modules/core/test/tools/serial/testSerialRead.cpp index 48c4bf434a..8054ddb8a5 100644 --- a/modules/core/test/tools/serial/testSerialRead.cpp +++ b/modules/core/test/tools/serial/testSerialRead.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test serial port communication. - * -*****************************************************************************/ + */ /*! \example testSerialRead.cpp @@ -88,4 +86,4 @@ int main(int argc, char **argv) std::cout << "Serial test is only working on unix-like OS." << std::endl; #endif return EXIT_SUCCESS; - } +} diff --git a/modules/core/test/tools/serial/testSerialWrite.cpp b/modules/core/test/tools/serial/testSerialWrite.cpp index 9b12439afc..92ab7d0118 100644 --- a/modules/core/test/tools/serial/testSerialWrite.cpp +++ b/modules/core/test/tools/serial/testSerialWrite.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test serial port communication. - * -*****************************************************************************/ + */ /*! \example testSerialWrite.cpp diff --git a/modules/core/test/tools/time/testTime.cpp b/modules/core/test/tools/time/testTime.cpp index c58fef4fb5..4aeb11482c 100644 --- a/modules/core/test/tools/time/testTime.cpp +++ b/modules/core/test/tools/time/testTime.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Time management. - * -*****************************************************************************/ + */ /*! diff --git a/modules/core/test/tools/xml/testXmlParser.cpp b/modules/core/test/tools/xml/testXmlParser.cpp index 23fbfb868e..98c41d3905 100644 --- a/modules/core/test/tools/xml/testXmlParser.cpp +++ b/modules/core/test/tools/xml/testXmlParser.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/detection/test/apriltag-with-dataset/perfApriltagDetection.cpp b/modules/detection/test/apriltag-with-dataset/perfApriltagDetection.cpp index 63c0903edd..27f4a26caf 100644 --- a/modules/detection/test/apriltag-with-dataset/perfApriltagDetection.cpp +++ b/modules/detection/test/apriltag-with-dataset/perfApriltagDetection.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,9 +29,11 @@ * * Description: * Apriltag detection performance test. - * -*****************************************************************************/ + */ +/*! + \example perfApriltagDetection.cpp + */ #include #if defined(VISP_HAVE_CATCH2) diff --git a/modules/detection/test/apriltag-with-dataset/testAprilTag.cpp b/modules/detection/test/apriltag-with-dataset/testAprilTag.cpp index 64ad274b6d..86b917aed3 100644 --- a/modules/detection/test/apriltag-with-dataset/testAprilTag.cpp +++ b/modules/detection/test/apriltag-with-dataset/testAprilTag.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test AprilTag detection. - * -*****************************************************************************/ + */ /*! \example testAprilTag.cpp diff --git a/modules/gui/test/display-with-dataset/testClick.cpp b/modules/gui/test/display-with-dataset/testClick.cpp index 48c0ce5d0e..fd617e0d09 100644 --- a/modules/gui/test/display-with-dataset/testClick.cpp +++ b/modules/gui/test/display-with-dataset/testClick.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test for mouse click manipulations. - * -*****************************************************************************/ + */ #include #include @@ -300,9 +298,9 @@ int main(int argc, const char **argv) std::cout << " No display is available\n"; } return EXIT_FAILURE; - } + } - // Get the option values + // Get the option values if (!opt_ipath.empty()) ipath = opt_ipath; @@ -448,12 +446,12 @@ int main(int argc, const char **argv) } } delete display; -} + } catch (...) { vpERROR_TRACE("Error while displaying the image"); return EXIT_FAILURE; } - } +} #else int main() { vpERROR_TRACE("You do not have display functionalities..."); } diff --git a/modules/gui/test/display-with-dataset/testDisplayScaled.cpp b/modules/gui/test/display-with-dataset/testDisplayScaled.cpp index 687fed7a53..8fb37971ac 100644 --- a/modules/gui/test/display-with-dataset/testDisplayScaled.cpp +++ b/modules/gui/test/display-with-dataset/testDisplayScaled.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,9 +29,11 @@ * * Description: * Display testing. - * -*****************************************************************************/ + */ +/*! + \example testDisplayScaled.cpp + */ #include #include diff --git a/modules/gui/test/display-with-dataset/testMouseEvent.cpp b/modules/gui/test/display-with-dataset/testMouseEvent.cpp index dec8c89a33..f00753ea9e 100644 --- a/modules/gui/test/display-with-dataset/testMouseEvent.cpp +++ b/modules/gui/test/display-with-dataset/testMouseEvent.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,9 +29,9 @@ * * Description: * Read an image sequence from the disk and display it. - * -*****************************************************************************/ + */ /*! + \file testMouseEvent.cpp \brief Read an image sequence from the disk and display it. @@ -372,7 +371,7 @@ int main(int argc, const char **argv) std::cout << " No display is available\n"; } return EXIT_FAILURE; -} + } if (!opt_display) opt_click_blocking = false; // turn off the waiting diff --git a/modules/gui/test/display-with-dataset/testVideoDevice.cpp b/modules/gui/test/display-with-dataset/testVideoDevice.cpp index 45429ff34b..87f85f86ef 100644 --- a/modules/gui/test/display-with-dataset/testVideoDevice.cpp +++ b/modules/gui/test/display-with-dataset/testVideoDevice.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,13 @@ * * Description: * Test for image display. - * - * Authors: - * Anthony Saunier - * -*****************************************************************************/ + */ + +/*! + \example testVideoDevice.cpp + + \brief Image display manipulations. +*/ #include #include @@ -56,13 +57,6 @@ #include #include -/*! - \example testVideoDevice.cpp - - \brief Image display manipulations. - -*/ - // List of allowed command line options #define GETOPTARGS "i:hlt:dc" @@ -298,9 +292,9 @@ int main(int argc, const char **argv) std::cout << " No display is available\n"; } return EXIT_FAILURE; - } + } - // Get the option values + // Get the option values if (!opt_ipath.empty()) ipath = opt_ipath; @@ -477,12 +471,12 @@ int main(int argc, const char **argv) vpDisplay::getClick(Irgba); } delete display; -} + } catch (...) { vpERROR_TRACE("Error while displaying the image"); return EXIT_FAILURE; } - } +} #else int main() { vpERROR_TRACE("You do not have display functionalities..."); } diff --git a/modules/gui/test/display/testDisplayPolygonLines.cpp b/modules/gui/test/display/testDisplayPolygonLines.cpp index d19cc90001..8b34ce672d 100644 --- a/modules/gui/test/display/testDisplayPolygonLines.cpp +++ b/modules/gui/test/display/testDisplayPolygonLines.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test display polygon lines - * -*****************************************************************************/ + */ /*! \example testDisplayPolygonLines.cpp diff --git a/modules/gui/test/display/testDisplayRoi.cpp b/modules/gui/test/display/testDisplayRoi.cpp index 052fe857ea..0fb808f707 100644 --- a/modules/gui/test/display/testDisplayRoi.cpp +++ b/modules/gui/test/display/testDisplayRoi.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test for image roi display. - * -*****************************************************************************/ + */ /*! \example testDisplayRoi.cpp @@ -207,10 +205,10 @@ int main(int argc, const char **argv) std::cout << "A click in the image to exit..." << std::endl; vpDisplay::getClick(C); } - } + } #else (void)argc; (void)argv; #endif return EXIT_SUCCESS; - } +} diff --git a/modules/gui/test/display/testDisplays.cpp b/modules/gui/test/display/testDisplays.cpp index 75b81698ac..b16c1dbe7e 100644 --- a/modules/gui/test/display/testDisplays.cpp +++ b/modules/gui/test/display/testDisplays.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,13 @@ * * Description: * Test for image display. - * - * Authors: - * Anthony Saunier - * -*****************************************************************************/ + */ + +/*! + \example testDisplays.cpp + + \brief Test all the displays. Draws several shapes. +*/ #include #include @@ -57,12 +58,6 @@ #include #include -/*! - \example testDisplays.cpp - - \brief Test all the displays. Draws several shapes. -*/ - // List of allowed command line options #define GETOPTARGS "hldc" @@ -419,16 +414,16 @@ int main(int argc, const char **argv) std::cout << " No display is available\n"; } return EXIT_FAILURE; - } + } - // Create a color image for each display. + // Create a color image for each display. runTest(opt_display, opt_click_allowed); // Create a grayscale image for each display. runTest(opt_display, opt_click_allowed); return EXIT_SUCCESS; -} + } catch (const vpException &e) { std::cout << "Catch an exception: " << e.getMessage() << std::endl; return EXIT_FAILURE; diff --git a/modules/imgproc/test/with-dataset/testAutoThreshold.cpp b/modules/imgproc/test/with-dataset/testAutoThreshold.cpp index f9aba1768c..a948a99e6d 100644 --- a/modules/imgproc/test/with-dataset/testAutoThreshold.cpp +++ b/modules/imgproc/test/with-dataset/testAutoThreshold.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/imgproc/test/with-dataset/testConnectedComponents.cpp b/modules/imgproc/test/with-dataset/testConnectedComponents.cpp index d0c8ac64d2..a3f40a21cd 100644 --- a/modules/imgproc/test/with-dataset/testConnectedComponents.cpp +++ b/modules/imgproc/test/with-dataset/testConnectedComponents.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/imgproc/test/with-dataset/testContours.cpp b/modules/imgproc/test/with-dataset/testContours.cpp index 9d11831de4..ddb2a52790 100644 --- a/modules/imgproc/test/with-dataset/testContours.cpp +++ b/modules/imgproc/test/with-dataset/testContours.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/imgproc/test/with-dataset/testFloodFill.cpp b/modules/imgproc/test/with-dataset/testFloodFill.cpp index f8b6d4be96..a51d84379d 100644 --- a/modules/imgproc/test/with-dataset/testFloodFill.cpp +++ b/modules/imgproc/test/with-dataset/testFloodFill.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/imgproc/test/with-dataset/testImgproc.cpp b/modules/imgproc/test/with-dataset/testImgproc.cpp index 6af19a1427..0a84ecbe76 100644 --- a/modules/imgproc/test/with-dataset/testImgproc.cpp +++ b/modules/imgproc/test/with-dataset/testImgproc.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/io/test/image-with-dataset/perfImageLoadSave.cpp b/modules/io/test/image-with-dataset/perfImageLoadSave.cpp index 3a039d903f..9237a59680 100644 --- a/modules/io/test/image-with-dataset/perfImageLoadSave.cpp +++ b/modules/io/test/image-with-dataset/perfImageLoadSave.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,11 @@ * * Description: * Benchmark color image conversion. - * -*****************************************************************************/ + */ + +/*! + \example perfImageLoadSave.cpp + */ #include diff --git a/modules/io/test/image-with-dataset/testImageLoadSave.cpp b/modules/io/test/image-with-dataset/testImageLoadSave.cpp index 72b8190888..c768a00951 100644 --- a/modules/io/test/image-with-dataset/testImageLoadSave.cpp +++ b/modules/io/test/image-with-dataset/testImageLoadSave.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,11 @@ * * Description: * Check that the different image I/O backends work correctly. - * -*****************************************************************************/ + */ + +/*! + \example testImageLoadSave.cpp + */ #include diff --git a/modules/io/test/testJsonArgumentParser.cpp b/modules/io/test/testJsonArgumentParser.cpp index 9a783aea74..9678b4b205 100644 --- a/modules/io/test/testJsonArgumentParser.cpp +++ b/modules/io/test/testJsonArgumentParser.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test vpJsonArgumentParser - * -*****************************************************************************/ + */ /*! \file testJsonArgumentParser.cpp diff --git a/modules/io/test/video/testVideo.cpp b/modules/io/test/video/testVideo.cpp index 734bc4e96f..c96e83bf46 100644 --- a/modules/io/test/video/testVideo.cpp +++ b/modules/io/test/video/testVideo.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test video i/o. - * -*****************************************************************************/ + */ /*! \file testVideo.cpp diff --git a/modules/robot/test/bebop2/testRobotBebop2.cpp b/modules/robot/test/bebop2/testRobotBebop2.cpp index efb708f6c6..eb2b5bfa36 100644 --- a/modules/robot/test/bebop2/testRobotBebop2.cpp +++ b/modules/robot/test/bebop2/testRobotBebop2.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,7 @@ * * Description: * Interface for the Irisa's Afma6 robot. - * - * Authors: - * Gatien Gaumerais - * -*****************************************************************************/ + */ /*! \example testRobotBebop2.cpp diff --git a/modules/robot/test/qbdevice/testQbSoftHand.cpp b/modules/robot/test/qbdevice/testQbSoftHand.cpp index 0eaacccd47..9906a703a0 100644 --- a/modules/robot/test/qbdevice/testQbSoftHand.cpp +++ b/modules/robot/test/qbdevice/testQbSoftHand.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test for qbdevice. - * -*****************************************************************************/ + */ /*! \example testQbSoftHand.cpp diff --git a/modules/robot/test/servo-afma4/testAfma4.cpp b/modules/robot/test/servo-afma4/testAfma4.cpp index 298b3b78a0..abb71e9e4d 100644 --- a/modules/robot/test/servo-afma4/testAfma4.cpp +++ b/modules/robot/test/servo-afma4/testAfma4.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -29,9 +28,8 @@ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. * * Description: - * Test for Afma 6 dof robot. - * -*****************************************************************************/ + * Test for Afma 4 dof robot. + */ /*! \example testAfma4.cpp diff --git a/modules/robot/test/servo-afma4/testRobotAfma4.cpp b/modules/robot/test/servo-afma4/testRobotAfma4.cpp index 8e65e317ce..6580bcc92e 100644 --- a/modules/robot/test/servo-afma4/testRobotAfma4.cpp +++ b/modules/robot/test/servo-afma4/testRobotAfma4.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test for Afma 4 dof robot. - * -*****************************************************************************/ + */ /*! \example testRobotAfma4.cpp diff --git a/modules/robot/test/servo-afma6/testAfma6.cpp b/modules/robot/test/servo-afma6/testAfma6.cpp index 257d5146d6..7078ff62cb 100644 --- a/modules/robot/test/servo-afma6/testAfma6.cpp +++ b/modules/robot/test/servo-afma6/testAfma6.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test for Afma 6 dof robot. - * -*****************************************************************************/ + */ /*! \example testAfma6.cpp diff --git a/modules/robot/test/servo-afma6/testRobotAfma6.cpp b/modules/robot/test/servo-afma6/testRobotAfma6.cpp index 1630015582..204a98fbbf 100644 --- a/modules/robot/test/servo-afma6/testRobotAfma6.cpp +++ b/modules/robot/test/servo-afma6/testRobotAfma6.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test for Afma 6 dof robot. - * -*****************************************************************************/ + */ /*! \example testRobotAfma6.cpp diff --git a/modules/robot/test/servo-afma6/testRobotAfma6Pose.cpp b/modules/robot/test/servo-afma6/testRobotAfma6Pose.cpp index 550171b4bd..8be72a5eb9 100644 --- a/modules/robot/test/servo-afma6/testRobotAfma6Pose.cpp +++ b/modules/robot/test/servo-afma6/testRobotAfma6Pose.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test for Afma 6 dof robot. - * -*****************************************************************************/ + */ /*! \example testRobotAfma6Pose.cpp diff --git a/modules/robot/test/servo-flir-ptu/testRobotFlirPtu.cpp b/modules/robot/test/servo-flir-ptu/testRobotFlirPtu.cpp index a7f55c3f4f..3f90de0e38 100644 --- a/modules/robot/test/servo-flir-ptu/testRobotFlirPtu.cpp +++ b/modules/robot/test/servo-flir-ptu/testRobotFlirPtu.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test FLIR PTU interface. - * -*****************************************************************************/ + */ /*! \example testRobotFlirPtu.cpp diff --git a/modules/robot/test/servo-franka/testFrankaCartForceTorque-2.cpp b/modules/robot/test/servo-franka/testFrankaCartForceTorque-2.cpp index 5003faf60a..36c628bdff 100644 --- a/modules/robot/test/servo-franka/testFrankaCartForceTorque-2.cpp +++ b/modules/robot/test/servo-franka/testFrankaCartForceTorque-2.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test Franka robot behavior - * -*****************************************************************************/ + */ /*! \example testFrankaCartForceTorque-2.cpp diff --git a/modules/robot/test/servo-franka/testFrankaCartForceTorque.cpp b/modules/robot/test/servo-franka/testFrankaCartForceTorque.cpp index f20f4a4c4d..d96ca155cc 100644 --- a/modules/robot/test/servo-franka/testFrankaCartForceTorque.cpp +++ b/modules/robot/test/servo-franka/testFrankaCartForceTorque.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test Franka robot behavior - * -*****************************************************************************/ + */ /*! \example testFrankaCartForceTorque.cpp diff --git a/modules/robot/test/servo-franka/testFrankaCartVelocity-2.cpp b/modules/robot/test/servo-franka/testFrankaCartVelocity-2.cpp index 532cf35159..c63ea2aff0 100644 --- a/modules/robot/test/servo-franka/testFrankaCartVelocity-2.cpp +++ b/modules/robot/test/servo-franka/testFrankaCartVelocity-2.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -29,9 +28,8 @@ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. * * Description: - * Test Franka robot behavior - * -*****************************************************************************/ + * Test Franka robot behavior + */ /*! \example testFrankaCartVelocity-2.cpp diff --git a/modules/robot/test/servo-franka/testFrankaCartVelocity-3.cpp b/modules/robot/test/servo-franka/testFrankaCartVelocity-3.cpp index 2c0de38e77..50eb6849f2 100644 --- a/modules/robot/test/servo-franka/testFrankaCartVelocity-3.cpp +++ b/modules/robot/test/servo-franka/testFrankaCartVelocity-3.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -29,9 +28,8 @@ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. * * Description: - * Test Franka robot behavior - * -*****************************************************************************/ + * Test Franka robot behavior + */ /*! \example testFrankaCartVelocity-3.cpp diff --git a/modules/robot/test/servo-franka/testFrankaCartVelocity.cpp b/modules/robot/test/servo-franka/testFrankaCartVelocity.cpp index f24d927899..4f9b473bc3 100644 --- a/modules/robot/test/servo-franka/testFrankaCartVelocity.cpp +++ b/modules/robot/test/servo-franka/testFrankaCartVelocity.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -29,9 +28,8 @@ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. * * Description: - * Test Franka robot behavior - * -*****************************************************************************/ + * Test Franka robot behavior + */ /*! \example testFrankaCartVelocity.cpp diff --git a/modules/robot/test/servo-franka/testFrankaGetPose.cpp b/modules/robot/test/servo-franka/testFrankaGetPose.cpp index 5a356bf0f2..a16d6456d6 100644 --- a/modules/robot/test/servo-franka/testFrankaGetPose.cpp +++ b/modules/robot/test/servo-franka/testFrankaGetPose.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -29,9 +28,8 @@ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. * * Description: - * Test Franka robot behavior - * -*****************************************************************************/ + * Test Franka robot behavior + */ /*! \example testFrankaGetPose.cpp diff --git a/modules/robot/test/servo-franka/testFrankaJointPosition.cpp b/modules/robot/test/servo-franka/testFrankaJointPosition.cpp index 6d49f7741c..265f9fab55 100644 --- a/modules/robot/test/servo-franka/testFrankaJointPosition.cpp +++ b/modules/robot/test/servo-franka/testFrankaJointPosition.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -29,9 +28,8 @@ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. * * Description: - * Test Franka robot behavior - * -*****************************************************************************/ + * Test Franka robot behavior + */ /*! \example testFrankaJointPosition.cpp diff --git a/modules/robot/test/servo-franka/testFrankaJointTorque.cpp b/modules/robot/test/servo-franka/testFrankaJointTorque.cpp index 6a373e73f7..bc6301bc30 100644 --- a/modules/robot/test/servo-franka/testFrankaJointTorque.cpp +++ b/modules/robot/test/servo-franka/testFrankaJointTorque.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -29,9 +28,8 @@ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. * * Description: - * Test Franka robot behavior - * -*****************************************************************************/ + * Test Franka robot behavior + */ /*! \example testFrankaJointTorque.cpp diff --git a/modules/robot/test/servo-franka/testFrankaJointVelocity-2.cpp b/modules/robot/test/servo-franka/testFrankaJointVelocity-2.cpp index a10e021803..d74bbd9677 100644 --- a/modules/robot/test/servo-franka/testFrankaJointVelocity-2.cpp +++ b/modules/robot/test/servo-franka/testFrankaJointVelocity-2.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -29,9 +28,8 @@ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. * * Description: - * Test Franka robot behavior - * -*****************************************************************************/ + * Test Franka robot behavior + */ /*! \example testFrankaJointVelocity-2.cpp diff --git a/modules/robot/test/servo-franka/testFrankaJointVelocity-3.cpp b/modules/robot/test/servo-franka/testFrankaJointVelocity-3.cpp index 39698a41b2..42c510dd2b 100644 --- a/modules/robot/test/servo-franka/testFrankaJointVelocity-3.cpp +++ b/modules/robot/test/servo-franka/testFrankaJointVelocity-3.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -29,9 +28,8 @@ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. * * Description: - * Test Franka robot behavior - * -*****************************************************************************/ + * Test Franka robot behavior + */ /*! \example testFrankaJointVelocity-3.cpp diff --git a/modules/robot/test/servo-franka/testFrankaJointVelocity.cpp b/modules/robot/test/servo-franka/testFrankaJointVelocity.cpp index 8f543f6704..947737b09f 100644 --- a/modules/robot/test/servo-franka/testFrankaJointVelocity.cpp +++ b/modules/robot/test/servo-franka/testFrankaJointVelocity.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -29,9 +28,8 @@ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. * * Description: - * Test Franka robot behavior - * -*****************************************************************************/ + * Test Franka robot behavior + */ /*! \example testFrankaJointVelocity.cpp diff --git a/modules/robot/test/servo-franka/testFrankaJointVelocityLimits.cpp b/modules/robot/test/servo-franka/testFrankaJointVelocityLimits.cpp index e895283946..16be5cd518 100644 --- a/modules/robot/test/servo-franka/testFrankaJointVelocityLimits.cpp +++ b/modules/robot/test/servo-franka/testFrankaJointVelocityLimits.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -29,9 +28,8 @@ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. * * Description: - * Test Franka robot behavior - * -*****************************************************************************/ + * Test Franka robot behavior + */ /*! \example testFrankaJointVelocityLimits.cpp diff --git a/modules/robot/test/servo-pixhawk/testPixhawkDroneKeyboard.cpp b/modules/robot/test/servo-pixhawk/testPixhawkDroneKeyboard.cpp index bbf582366e..00298b1874 100644 --- a/modules/robot/test/servo-pixhawk/testPixhawkDroneKeyboard.cpp +++ b/modules/robot/test/servo-pixhawk/testPixhawkDroneKeyboard.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test to control from keyboard a drone equipped with a Pixhawk thanks to mavsdk. - * -*****************************************************************************/ + */ /*! * \example testPixhawkDroneKeyboard.cpp diff --git a/modules/robot/test/servo-pixhawk/testPixhawkDronePositionAbsoluteControl.cpp b/modules/robot/test/servo-pixhawk/testPixhawkDronePositionAbsoluteControl.cpp index a07cab8a38..164ee8c0df 100644 --- a/modules/robot/test/servo-pixhawk/testPixhawkDronePositionAbsoluteControl.cpp +++ b/modules/robot/test/servo-pixhawk/testPixhawkDronePositionAbsoluteControl.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,8 +30,7 @@ * Description: * Simple example to demonstrate how to control in position using mavsdk * a drone equipped with a Pixhawk connected to a Jetson TX2. - * -*****************************************************************************/ + */ /*! * \example testPixhawkDronePositionAbsoluteControl.cpp diff --git a/modules/robot/test/servo-pixhawk/testPixhawkDronePositionRelativeControl.cpp b/modules/robot/test/servo-pixhawk/testPixhawkDronePositionRelativeControl.cpp index 160a9842bc..a46324f516 100644 --- a/modules/robot/test/servo-pixhawk/testPixhawkDronePositionRelativeControl.cpp +++ b/modules/robot/test/servo-pixhawk/testPixhawkDronePositionRelativeControl.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,8 +30,7 @@ * Description: * Simple example to demonstrate how to control in position using mavsdk * a drone equipped with a Pixhawk connected to a Jetson TX2. - * -*****************************************************************************/ + */ /*! * \example testPixhawkDronePositionRelativeControl.cpp diff --git a/modules/robot/test/servo-pixhawk/testPixhawkDroneTakeoff.cpp b/modules/robot/test/servo-pixhawk/testPixhawkDroneTakeoff.cpp index d3b6fc4bc2..bb286867f1 100644 --- a/modules/robot/test/servo-pixhawk/testPixhawkDroneTakeoff.cpp +++ b/modules/robot/test/servo-pixhawk/testPixhawkDroneTakeoff.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,8 +30,7 @@ * Description: * Simple example to demonstrate how takeoff and land using mavsdk on * a drone equipped with a Pixhawk connected to a Jetson TX2. - * -*****************************************************************************/ + */ /*! * \example testPixhawkDroneTakeoff.cpp diff --git a/modules/robot/test/servo-pixhawk/testPixhawkDroneVelocityControl.cpp b/modules/robot/test/servo-pixhawk/testPixhawkDroneVelocityControl.cpp index a5424fe836..5b51dd8ceb 100644 --- a/modules/robot/test/servo-pixhawk/testPixhawkDroneVelocityControl.cpp +++ b/modules/robot/test/servo-pixhawk/testPixhawkDroneVelocityControl.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,8 +30,7 @@ * Description: * Simple example to demonstrate how to control in velocity using mavsdk * a drone equipped with a Pixhawk connected to a Jetson TX2. - * -*****************************************************************************/ + */ /*! * \example testPixhawkDroneVelocityControl.cpp diff --git a/modules/robot/test/servo-pixhawk/testPixhawkRoverVelocityControl.cpp b/modules/robot/test/servo-pixhawk/testPixhawkRoverVelocityControl.cpp index bb83c1fd9f..2f5ff54099 100644 --- a/modules/robot/test/servo-pixhawk/testPixhawkRoverVelocityControl.cpp +++ b/modules/robot/test/servo-pixhawk/testPixhawkRoverVelocityControl.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,8 +30,7 @@ * Description: * Simple example to demonstrate how to control in velocity using mavsdk * a drone equipped with a Pixhawk connected to a Jetson TX2. - * -*****************************************************************************/ + */ /*! * \example testPixhawkRoverVelocityControl.cpp diff --git a/modules/robot/test/servo-pololu/testPololuPosition.cpp b/modules/robot/test/servo-pololu/testPololuPosition.cpp index 46d526aa69..8f17c828fa 100644 --- a/modules/robot/test/servo-pololu/testPololuPosition.cpp +++ b/modules/robot/test/servo-pololu/testPololuPosition.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/robot/test/servo-pololu/testPololuVelocity.cpp b/modules/robot/test/servo-pololu/testPololuVelocity.cpp index 01f23a7427..51f22a80db 100644 --- a/modules/robot/test/servo-pololu/testPololuVelocity.cpp +++ b/modules/robot/test/servo-pololu/testPololuVelocity.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/robot/test/servo-universal-robots/testUniversalRobotsCartPosition.cpp b/modules/robot/test/servo-universal-robots/testUniversalRobotsCartPosition.cpp index 95e87ae733..8d45173727 100644 --- a/modules/robot/test/servo-universal-robots/testUniversalRobotsCartPosition.cpp +++ b/modules/robot/test/servo-universal-robots/testUniversalRobotsCartPosition.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test Universal Robot behavior. - * -*****************************************************************************/ + */ /*! \example testUniversalRobotsCartPosition.cpp diff --git a/modules/robot/test/servo-universal-robots/testUniversalRobotsCartVelocity.cpp b/modules/robot/test/servo-universal-robots/testUniversalRobotsCartVelocity.cpp index c28b4e0ae4..5f36e6e2f3 100644 --- a/modules/robot/test/servo-universal-robots/testUniversalRobotsCartVelocity.cpp +++ b/modules/robot/test/servo-universal-robots/testUniversalRobotsCartVelocity.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test Universal Robots robot behavior - * -*****************************************************************************/ + */ /*! \example testUniversalRobotsCartVelocity.cpp diff --git a/modules/robot/test/servo-universal-robots/testUniversalRobotsGetData.cpp b/modules/robot/test/servo-universal-robots/testUniversalRobotsGetData.cpp index aa4ecff68d..ae88ad566d 100644 --- a/modules/robot/test/servo-universal-robots/testUniversalRobotsGetData.cpp +++ b/modules/robot/test/servo-universal-robots/testUniversalRobotsGetData.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test Universal Robots behavior - * -*****************************************************************************/ + */ /*! \example testUniversalRobotsGetData.cpp diff --git a/modules/robot/test/servo-universal-robots/testUniversalRobotsJointPosition.cpp b/modules/robot/test/servo-universal-robots/testUniversalRobotsJointPosition.cpp index 82b4225940..4274f5c1c3 100644 --- a/modules/robot/test/servo-universal-robots/testUniversalRobotsJointPosition.cpp +++ b/modules/robot/test/servo-universal-robots/testUniversalRobotsJointPosition.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test Universal Robot behavior. - * -*****************************************************************************/ + */ /*! \example testUniversalRobotsJointPosition.cpp diff --git a/modules/robot/test/servo-universal-robots/testUniversalRobotsJointVelocity.cpp b/modules/robot/test/servo-universal-robots/testUniversalRobotsJointVelocity.cpp index e5d5ac688b..ab1e9fa573 100644 --- a/modules/robot/test/servo-universal-robots/testUniversalRobotsJointVelocity.cpp +++ b/modules/robot/test/servo-universal-robots/testUniversalRobotsJointVelocity.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test Universal Robots robot behavior - * -*****************************************************************************/ + */ /*! \example testUniversalRobotsJointVelocity.cpp diff --git a/modules/robot/test/servo-viper/testRobotViper650-frames.cpp b/modules/robot/test/servo-viper/testRobotViper650-frames.cpp index e5f17dff01..a7efb25b26 100644 --- a/modules/robot/test/servo-viper/testRobotViper650-frames.cpp +++ b/modules/robot/test/servo-viper/testRobotViper650-frames.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test Viper 650 robot. - * -*****************************************************************************/ + */ /*! \example testRobotViper650-frames.cpp diff --git a/modules/robot/test/servo-viper/testRobotViper850-frames.cpp b/modules/robot/test/servo-viper/testRobotViper850-frames.cpp index 36095a38d1..e6343a03ac 100644 --- a/modules/robot/test/servo-viper/testRobotViper850-frames.cpp +++ b/modules/robot/test/servo-viper/testRobotViper850-frames.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test Viper 650 robot. - * -*****************************************************************************/ + */ /*! \example testRobotViper850-frames.cpp diff --git a/modules/robot/test/servo-viper/testRobotViper850.cpp b/modules/robot/test/servo-viper/testRobotViper850.cpp index 81905b60b5..876375c4c3 100644 --- a/modules/robot/test/servo-viper/testRobotViper850.cpp +++ b/modules/robot/test/servo-viper/testRobotViper850.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * Copyright (C) 2005 - 202 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 @@ -29,9 +28,8 @@ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. * * Description: - * Test for Afma 6 dof robot. - * -*****************************************************************************/ + * Test for Viper650 6 dof robot. + */ /*! \example testRobotViper850.cpp diff --git a/modules/robot/test/servo-viper/testRobotViper850Pose.cpp b/modules/robot/test/servo-viper/testRobotViper850Pose.cpp index d86eee79de..db0f60db43 100644 --- a/modules/robot/test/servo-viper/testRobotViper850Pose.cpp +++ b/modules/robot/test/servo-viper/testRobotViper850Pose.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -29,9 +28,8 @@ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. * * Description: - * Test for Afma 6 dof robot. - * -*****************************************************************************/ + * Test for Viper650 6 dof robot. + */ /*! \example testRobotViper850Pose.cpp diff --git a/modules/robot/test/servo-viper/testViper650.cpp b/modules/robot/test/servo-viper/testViper650.cpp index 9d378ade85..737ce46a9e 100644 --- a/modules/robot/test/servo-viper/testViper650.cpp +++ b/modules/robot/test/servo-viper/testViper650.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test for Viper650 6 dof robot. - * -*****************************************************************************/ + */ /*! \example testViper650.cpp diff --git a/modules/robot/test/servo-viper/testViper850.cpp b/modules/robot/test/servo-viper/testViper850.cpp index d358ff741a..0a0e0f3f09 100644 --- a/modules/robot/test/servo-viper/testViper850.cpp +++ b/modules/robot/test/servo-viper/testViper850.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test for Viper850 6 dof robot. - * -*****************************************************************************/ + */ /*! \example testViper850.cpp diff --git a/modules/robot/test/virtuose/testVirtuose.cpp b/modules/robot/test/virtuose/testVirtuose.cpp index 5e3e9cc635..40cbc46739 100644 --- a/modules/robot/test/virtuose/testVirtuose.cpp +++ b/modules/robot/test/virtuose/testVirtuose.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test for Virtuose SDK wrapper. - * -*****************************************************************************/ + */ /*! \example testVirtuose.cpp @@ -93,7 +91,7 @@ int main(int argc, char **argv) catch (const vpException &e) { std::cout << "Catch an exception: " << e.getStringMessage() << std::endl; return EXIT_FAILURE; -} + } #else (void)argc; (void)argv; diff --git a/modules/robot/test/virtuose/testVirtuoseAfma6.cpp b/modules/robot/test/virtuose/testVirtuoseAfma6.cpp index da2fabb95d..81371e67e1 100644 --- a/modules/robot/test/virtuose/testVirtuoseAfma6.cpp +++ b/modules/robot/test/virtuose/testVirtuoseAfma6.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,7 @@ * * Description: * Test for Virtuose SDK wrapper. - * - * Authors: - * Nicolò Pedemonte - * -*****************************************************************************/ + */ /*! \example testVirtuoseAfma6.cpp @@ -146,7 +141,7 @@ int main() catch (const vpException &e) { robot.stopMotion(); std::cout << "Catch an exception: " << e.getStringMessage() << std::endl; -} + } #else std::cout << "You should install Virtuose SDK to use this binary..." << std::endl; #endif diff --git a/modules/robot/test/virtuose/testVirtuoseHapticBox.cpp b/modules/robot/test/virtuose/testVirtuoseHapticBox.cpp index 0f59d69555..67f8cc8fbf 100644 --- a/modules/robot/test/virtuose/testVirtuoseHapticBox.cpp +++ b/modules/robot/test/virtuose/testVirtuoseHapticBox.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,12 +29,7 @@ * * Description: * Test for Virtuose SDK wrapper. - * - * Authors: - * Nicolò Pedemonte - * Firas Abi Farraj - * -*****************************************************************************/ + */ /*! \example testVirtuoseHapticBox.cpp diff --git a/modules/robot/test/virtuose/testVirtuoseJointLimits.cpp b/modules/robot/test/virtuose/testVirtuoseJointLimits.cpp index 1f468e66b5..ba0231050f 100644 --- a/modules/robot/test/virtuose/testVirtuoseJointLimits.cpp +++ b/modules/robot/test/virtuose/testVirtuoseJointLimits.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,11 +29,7 @@ * * Description: * Test for Virtuose SDK wrapper. - * - * Authors: - * Nicolò Pedemonte - * -*****************************************************************************/ + */ /*! \example testVirtuoseJointLimits.cpp diff --git a/modules/robot/test/virtuose/testVirtuosePeriodicFunction.cpp b/modules/robot/test/virtuose/testVirtuosePeriodicFunction.cpp index bc53164298..e42383fb3d 100644 --- a/modules/robot/test/virtuose/testVirtuosePeriodicFunction.cpp +++ b/modules/robot/test/virtuose/testVirtuosePeriodicFunction.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test for Virtuose SDK wrapper. - * -*****************************************************************************/ + */ /*! \example testVirtuosePeriodicFunction.cpp diff --git a/modules/robot/test/virtuose/testVirtuoseWithGlove.cpp b/modules/robot/test/virtuose/testVirtuoseWithGlove.cpp index 7c82ae79ba..6721648328 100644 --- a/modules/robot/test/virtuose/testVirtuoseWithGlove.cpp +++ b/modules/robot/test/virtuose/testVirtuoseWithGlove.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,9 +29,7 @@ * * Description: * Test for Virtuose + Glove SDK wrapper. - * - * -*****************************************************************************/ + */ /*! \example testVirtuoseWithGlove.cpp diff --git a/modules/sensor/test/force-torque/testComedi.cpp b/modules/sensor/test/force-torque/testComedi.cpp index d631ccd525..433793c9b8 100644 --- a/modules/sensor/test/force-torque/testComedi.cpp +++ b/modules/sensor/test/force-torque/testComedi.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2024 by Inria. All rights reserved. * diff --git a/modules/sensor/test/force-torque/testForceTorqueAti.cpp b/modules/sensor/test/force-torque/testForceTorqueAti.cpp index b664118a40..b7c9f2ca1f 100644 --- a/modules/sensor/test/force-torque/testForceTorqueAti.cpp +++ b/modules/sensor/test/force-torque/testForceTorqueAti.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test force/torque ATI sensor. - * -*****************************************************************************/ + */ /*! \example testForceTorqueAti.cpp @@ -146,7 +144,7 @@ void scopeFunction(std::mutex &mutex_data, std::mutex &mutex_state, t_shared_dat } std::cout << "End of scope thread" << std::endl; - } +} int main(int argc, char **argv) { diff --git a/modules/sensor/test/force-torque/testForceTorqueAtiNetFTSensor.cpp b/modules/sensor/test/force-torque/testForceTorqueAtiNetFTSensor.cpp index eba365c70d..b6b1b167fd 100644 --- a/modules/sensor/test/force-torque/testForceTorqueAtiNetFTSensor.cpp +++ b/modules/sensor/test/force-torque/testForceTorqueAtiNetFTSensor.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test force/torque ATI sensor. - * -*****************************************************************************/ + */ /*! \example testForceTorqueAtiNetFTSensor.cpp diff --git a/modules/sensor/test/force-torque/testForceTorqueIitSensor.cpp b/modules/sensor/test/force-torque/testForceTorqueIitSensor.cpp index 85960bf797..271e19b7cc 100644 --- a/modules/sensor/test/force-torque/testForceTorqueIitSensor.cpp +++ b/modules/sensor/test/force-torque/testForceTorqueIitSensor.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test force/torque IIT sensor. - * -*****************************************************************************/ + */ /*! \example testForceTorqueIitSensor.cpp diff --git a/modules/sensor/test/framegrabber/test1394TwoGrabber.cpp b/modules/sensor/test/framegrabber/test1394TwoGrabber.cpp index 5157b7f26b..4271297787 100644 --- a/modules/sensor/test/framegrabber/test1394TwoGrabber.cpp +++ b/modules/sensor/test/framegrabber/test1394TwoGrabber.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -33,9 +32,9 @@ */ /*! - \file test1394TwoGrabber.cpp + \example test1394TwoGrabber.cpp - \brief Aquire images using libdc1394-2.x library. + \brief Acquire images using libdc1394-2.x library. */ #include @@ -54,9 +53,6 @@ using namespace VISP_NAMESPACE_NAME; #include #include -/*! - \example test1394TwoGrabber.cpp -*/ int main() { try { diff --git a/modules/sensor/test/framegrabber/test1394TwoResetBus.cpp b/modules/sensor/test/framegrabber/test1394TwoResetBus.cpp index eaacbe8989..132d5a70c4 100644 --- a/modules/sensor/test/framegrabber/test1394TwoResetBus.cpp +++ b/modules/sensor/test/framegrabber/test1394TwoResetBus.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -33,9 +32,15 @@ */ /*! - \file test1394TwoResetBus.cpp + \example test1394TwoResetBus.cpp - \brief Resets the IEEE1394 bus using libdc1394-2.x library. + Resets the IEEE1394 bus which first camera is attached to. Resetting + the bus is "rude" to other devices because it causes them to + re-enumerate on the bus and may cause a temporary disruption in + their current activities. Thus, use it sparingly. Its primary use + is if a program shuts down uncleanly and needs to free leftover ISO + channels or bandwidth. A bus reset will free those things as a side + effect. */ #include @@ -53,17 +58,6 @@ using namespace VISP_NAMESPACE_NAME; #include #include -/*! - \example test1394TwoResetBus.cpp - - Resets the IEEE1394 bus which first camera is attached to. Resetting - the bus is "rude" to other devices because it causes them to - re-enumerate on the bus and may cause a temporary disruption in - their current activities. Thus, use it sparingly. Its primary use - is if a program shuts down uncleanly and needs to free leftover ISO - channels or bandwidth. A bus reset will free those things as a side - effect. -*/ int main() { try { diff --git a/modules/sensor/test/framegrabber/testPylonGrabber.cpp b/modules/sensor/test/framegrabber/testPylonGrabber.cpp index 569d189928..f4391652cb 100644 --- a/modules/sensor/test/framegrabber/testPylonGrabber.cpp +++ b/modules/sensor/test/framegrabber/testPylonGrabber.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,14 +29,10 @@ * * Description: * Basler cameras video capture using Pylon SDK. - * - * Authors: - * Wenfeng CAI - * -*****************************************************************************/ + */ /*! - \file testPylonGrabber.cpp + \example testPylonGrabber.cpp \brief Acquire images using Pylon library. */ @@ -53,9 +48,7 @@ #include #include #include -/*! - \example testPylonGrabber.cpp -*/ + int main() { #ifdef ENABLE_VISP_NAMESPACE diff --git a/modules/sensor/test/mocap/testMocapQualisys.cpp b/modules/sensor/test/mocap/testMocapQualisys.cpp index 599e879735..e026d2056d 100644 --- a/modules/sensor/test/mocap/testMocapQualisys.cpp +++ b/modules/sensor/test/mocap/testMocapQualisys.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test Qualisys Motion Capture System. - * -*****************************************************************************/ + */ /*! * \example testMocapQualisys.cpp diff --git a/modules/sensor/test/mocap/testMocapVicon.cpp b/modules/sensor/test/mocap/testMocapVicon.cpp index 614e270424..06a16f7909 100644 --- a/modules/sensor/test/mocap/testMocapVicon.cpp +++ b/modules/sensor/test/mocap/testMocapVicon.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test Vicon Motion Capture System. - * -*****************************************************************************/ + */ /*! * \example testMocapVicon.cpp diff --git a/modules/sensor/test/rgb-depth/testOccipitalStructure_Core_images.cpp b/modules/sensor/test/rgb-depth/testOccipitalStructure_Core_images.cpp index 7a9d40b793..7490fc8c64 100644 --- a/modules/sensor/test/rgb-depth/testOccipitalStructure_Core_images.cpp +++ b/modules/sensor/test/rgb-depth/testOccipitalStructure_Core_images.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Image acquisition with Structure Core sensor and libStructure. - * -*****************************************************************************/ + */ /*! \example testOccipitalStructure_Core_images.cpp diff --git a/modules/sensor/test/rgb-depth/testOccipitalStructure_Core_imu.cpp b/modules/sensor/test/rgb-depth/testOccipitalStructure_Core_imu.cpp index cd7eb66bfa..835129baa0 100644 --- a/modules/sensor/test/rgb-depth/testOccipitalStructure_Core_imu.cpp +++ b/modules/sensor/test/rgb-depth/testOccipitalStructure_Core_imu.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * IMU data acquisition with Structure Core sensor and libStructure. - * -*****************************************************************************/ + */ /*! \example testOccipitalStructure_Core_imu.cpp diff --git a/modules/sensor/test/rgb-depth/testOccipitalStructure_Core_pcl.cpp b/modules/sensor/test/rgb-depth/testOccipitalStructure_Core_pcl.cpp index 2bd8ff8efc..b4cdb4c262 100644 --- a/modules/sensor/test/rgb-depth/testOccipitalStructure_Core_pcl.cpp +++ b/modules/sensor/test/rgb-depth/testOccipitalStructure_Core_pcl.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Point cloud depth visualization with Occipital Structure Core sensor. - * -*****************************************************************************/ + */ /*! \example testOccipitalStructure_Core_pcl.cpp diff --git a/modules/sensor/test/rgb-depth/testRealSense2_D435.cpp b/modules/sensor/test/rgb-depth/testRealSense2_D435.cpp index cbd0b7848c..c85f988bc0 100644 --- a/modules/sensor/test/rgb-depth/testRealSense2_D435.cpp +++ b/modules/sensor/test/rgb-depth/testRealSense2_D435.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test Intel RealSense acquisition with librealsense2. - * -*****************************************************************************/ + */ /*! \example testRealSense2_D435.cpp Test Intel RealSense D435 acquisition with librealsense2. diff --git a/modules/sensor/test/rgb-depth/testRealSense2_D435_align.cpp b/modules/sensor/test/rgb-depth/testRealSense2_D435_align.cpp index 9c85d173b4..7c6ee4b573 100644 --- a/modules/sensor/test/rgb-depth/testRealSense2_D435_align.cpp +++ b/modules/sensor/test/rgb-depth/testRealSense2_D435_align.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test Intel RealSense D435 acquisition with librealsense2. - * -*****************************************************************************/ + */ /*! \example testRealSense2_D435_align.cpp Test Intel RealSense D435 acquisition with librealsense2. diff --git a/modules/sensor/test/rgb-depth/testRealSense2_D435_opencv.cpp b/modules/sensor/test/rgb-depth/testRealSense2_D435_opencv.cpp index ee66c39441..b58015fe2d 100644 --- a/modules/sensor/test/rgb-depth/testRealSense2_D435_opencv.cpp +++ b/modules/sensor/test/rgb-depth/testRealSense2_D435_opencv.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test Intel RealSense acquisition with librealsense2 (OpenCV demo). - * -*****************************************************************************/ + */ /*! \example testRealSense2_D435_opencv.cpp Test Intel RealSense D435 acquisition with librealsense2 (OpenCV demo). diff --git a/modules/sensor/test/rgb-depth/testRealSense2_D435_pcl.cpp b/modules/sensor/test/rgb-depth/testRealSense2_D435_pcl.cpp index f934a202ac..316b1704e5 100644 --- a/modules/sensor/test/rgb-depth/testRealSense2_D435_pcl.cpp +++ b/modules/sensor/test/rgb-depth/testRealSense2_D435_pcl.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test Intel RealSense acquisition with librealsense2 (PCL demo). - * -*****************************************************************************/ + */ /*! \example testRealSense2_D435_pcl.cpp Test Intel RealSense D435 acquisition with librealsense2 (PCL demo). diff --git a/modules/sensor/test/rgb-depth/testRealSense2_SR300.cpp b/modules/sensor/test/rgb-depth/testRealSense2_SR300.cpp index 73911e5884..41c50c9947 100644 --- a/modules/sensor/test/rgb-depth/testRealSense2_SR300.cpp +++ b/modules/sensor/test/rgb-depth/testRealSense2_SR300.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test Intel RealSense acquisition with librealsense2. - * -*****************************************************************************/ + */ /*! \example testRealSense2_SR300.cpp diff --git a/modules/sensor/test/rgb-depth/testRealSense2_T265_images.cpp b/modules/sensor/test/rgb-depth/testRealSense2_T265_images.cpp index 35f90ba403..1cc6173940 100644 --- a/modules/sensor/test/rgb-depth/testRealSense2_T265_images.cpp +++ b/modules/sensor/test/rgb-depth/testRealSense2_T265_images.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Images acquisition with RealSense T265 sensor and librealsense2. - * -*****************************************************************************/ + */ /*! \example testRealSense2_T265_images.cpp diff --git a/modules/sensor/test/rgb-depth/testRealSense2_T265_images_odometry.cpp b/modules/sensor/test/rgb-depth/testRealSense2_T265_images_odometry.cpp index 91f62efa5f..2e50a9bc47 100644 --- a/modules/sensor/test/rgb-depth/testRealSense2_T265_images_odometry.cpp +++ b/modules/sensor/test/rgb-depth/testRealSense2_T265_images_odometry.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,8 +30,7 @@ * Description: * Acquisition of images and odometry information with RealSense T265 sensor * and librealsense2. - * -*****************************************************************************/ + */ /*! \example testRealSense2_T265_images_odometry.cpp diff --git a/modules/sensor/test/rgb-depth/testRealSense2_T265_images_odometry_async.cpp b/modules/sensor/test/rgb-depth/testRealSense2_T265_images_odometry_async.cpp index 19cad8c1ef..11201305d7 100644 --- a/modules/sensor/test/rgb-depth/testRealSense2_T265_images_odometry_async.cpp +++ b/modules/sensor/test/rgb-depth/testRealSense2_T265_images_odometry_async.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,8 +30,7 @@ * Description: * Asynchronous acquisition of images and odometry information with * RealSense T265 sensor and librealsense2. - * -*****************************************************************************/ + */ /*! \example testRealSense2_T265_images_odometry_async.cpp diff --git a/modules/sensor/test/rgb-depth/testRealSense2_T265_imu.cpp b/modules/sensor/test/rgb-depth/testRealSense2_T265_imu.cpp index 104b458484..81769a737d 100644 --- a/modules/sensor/test/rgb-depth/testRealSense2_T265_imu.cpp +++ b/modules/sensor/test/rgb-depth/testRealSense2_T265_imu.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Acquisition of IMU data with RealSense T265 sensor and librealsense2. - * -*****************************************************************************/ + */ /*! \example testRealSense2_T265_imu.cpp diff --git a/modules/sensor/test/rgb-depth/testRealSense2_T265_odometry.cpp b/modules/sensor/test/rgb-depth/testRealSense2_T265_odometry.cpp index 4f71c7bcfe..a72532b197 100644 --- a/modules/sensor/test/rgb-depth/testRealSense2_T265_odometry.cpp +++ b/modules/sensor/test/rgb-depth/testRealSense2_T265_odometry.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Acquisition of odometry data with RealSense T265 sensor and librealsense2. - * -*****************************************************************************/ + */ /*! \example testRealSense2_T265_odometry.cpp diff --git a/modules/sensor/test/rgb-depth/testRealSense2_T265_undistort.cpp b/modules/sensor/test/rgb-depth/testRealSense2_T265_undistort.cpp index 1a47dcd07d..4201924af8 100644 --- a/modules/sensor/test/rgb-depth/testRealSense2_T265_undistort.cpp +++ b/modules/sensor/test/rgb-depth/testRealSense2_T265_undistort.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,9 +29,8 @@ * * Description: * Image acquisition with RealSense T265 sensor and librealsense2 and - * undistorting it using vpImageTools - * -*****************************************************************************/ + * undistorting it using vpImageTools. + */ /*! \example testRealSense2_T265_undistort.cpp diff --git a/modules/tracker/blob/test/tracking-with-dataset/testTrackDot.cpp b/modules/tracker/blob/test/tracking-with-dataset/testTrackDot.cpp index 443658b5c1..16df3c5de0 100644 --- a/modules/tracker/blob/test/tracking-with-dataset/testTrackDot.cpp +++ b/modules/tracker/blob/test/tracking-with-dataset/testTrackDot.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test auto detection of dots. - * -*****************************************************************************/ + */ #include #include diff --git a/modules/tracker/mbt/test/generic-with-dataset/perfGenericTracker.cpp b/modules/tracker/mbt/test/generic-with-dataset/perfGenericTracker.cpp index a89da3b411..ada26c19e3 100644 --- a/modules/tracker/mbt/test/generic-with-dataset/perfGenericTracker.cpp +++ b/modules/tracker/mbt/test/generic-with-dataset/perfGenericTracker.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,9 +29,11 @@ * * Description: * Benchmark generic tracker. - * -*****************************************************************************/ + */ +/*! + \example perfGenericTracker.cpp + */ #include #if defined(VISP_HAVE_CATCH2) diff --git a/modules/tracker/mbt/test/generic-with-dataset/testGenericTracker.cpp b/modules/tracker/mbt/test/generic-with-dataset/testGenericTracker.cpp index 47d4a6cf99..549fc046ba 100644 --- a/modules/tracker/mbt/test/generic-with-dataset/testGenericTracker.cpp +++ b/modules/tracker/mbt/test/generic-with-dataset/testGenericTracker.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Regression test for MBT. - * -*****************************************************************************/ + */ /*! \example testGenericTracker.cpp @@ -713,7 +711,7 @@ int main(int argc, const char *argv[]) "KLT module or OpenCV is not available.\nTest is not run." << std::endl; return EXIT_SUCCESS; - } + } #endif // Test if an input path is set @@ -746,7 +744,7 @@ int main(int argc, const char *argv[]) std::cout << "Test succeed" << std::endl; return EXIT_SUCCESS; -} + } catch (const vpException &e) { std::cout << "Catch an exception: " << e << std::endl; return EXIT_FAILURE; diff --git a/modules/tracker/mbt/test/generic-with-dataset/testGenericTrackerCAOParsing.cpp b/modules/tracker/mbt/test/generic-with-dataset/testGenericTrackerCAOParsing.cpp index 6dbf3e7668..501db864b6 100644 --- a/modules/tracker/mbt/test/generic-with-dataset/testGenericTrackerCAOParsing.cpp +++ b/modules/tracker/mbt/test/generic-with-dataset/testGenericTrackerCAOParsing.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,11 @@ * * Description: * Test MBT CAO parsing. - * -*****************************************************************************/ + */ + +/*! + \example testGenericTrackerCAOParsing.cpp + */ #include diff --git a/modules/tracker/mbt/test/generic-with-dataset/testGenericTrackerDepth.cpp b/modules/tracker/mbt/test/generic-with-dataset/testGenericTrackerDepth.cpp index 1f8626364b..b382e990f3 100644 --- a/modules/tracker/mbt/test/generic-with-dataset/testGenericTrackerDepth.cpp +++ b/modules/tracker/mbt/test/generic-with-dataset/testGenericTrackerDepth.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Regression test for depth MBT. - * -*****************************************************************************/ + */ /*! \example testGenericTrackerDepth.cpp diff --git a/modules/tracker/mbt/test/generic-with-dataset/testGenericTrackerDeterminist.cpp b/modules/tracker/mbt/test/generic-with-dataset/testGenericTrackerDeterminist.cpp index 12c818da42..6e9d44ac4f 100644 --- a/modules/tracker/mbt/test/generic-with-dataset/testGenericTrackerDeterminist.cpp +++ b/modules/tracker/mbt/test/generic-with-dataset/testGenericTrackerDeterminist.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,11 @@ * * Description: * Check that MBT is deterministic. - * -*****************************************************************************/ + */ + +/*! + \example testGenericTrackerDeterminist.cpp + */ #include diff --git a/modules/tracker/mbt/test/generic-with-dataset/testMbtJsonSettings.cpp b/modules/tracker/mbt/test/generic-with-dataset/testMbtJsonSettings.cpp index 614488462b..93bde77f0f 100644 --- a/modules/tracker/mbt/test/generic-with-dataset/testMbtJsonSettings.cpp +++ b/modules/tracker/mbt/test/generic-with-dataset/testMbtJsonSettings.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test vpMbGenericTracker JSON parse / save. - * -*****************************************************************************/ + */ /*! \file testMbtJsonSettings.cpp diff --git a/modules/tracker/mbt/test/generic-with-dataset/testMbtXmlGenericParser.cpp b/modules/tracker/mbt/test/generic-with-dataset/testMbtXmlGenericParser.cpp index ad109fa5dc..fac74b7cde 100644 --- a/modules/tracker/mbt/test/generic-with-dataset/testMbtXmlGenericParser.cpp +++ b/modules/tracker/mbt/test/generic-with-dataset/testMbtXmlGenericParser.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test vpMbtXmlGenericParser parse / save. - * -*****************************************************************************/ + */ /*! \file testMbtXmlGenericParser.cpp @@ -159,7 +157,7 @@ int main() return EXIT_FAILURE; } } - } + } #elif !(defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV)) std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl; #elif !(defined(VISP_HAVE_PUGIXML)) @@ -168,4 +166,4 @@ int main() std::cout << "Test succeed" << std::endl; return EXIT_SUCCESS; - } +} diff --git a/modules/tracker/mbt/test/testTukeyEstimator.cpp b/modules/tracker/mbt/test/testTukeyEstimator.cpp index b6ad7c55e0..b44e6eb9f0 100644 --- a/modules/tracker/mbt/test/testTukeyEstimator.cpp +++ b/modules/tracker/mbt/test/testTukeyEstimator.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,7 @@ * * Description: * Test Tukey M-Estimator. - * -*****************************************************************************/ + */ /*! \example testTukeyEstimator.cpp diff --git a/modules/tracker/me/test/testJsonMe.cpp b/modules/tracker/me/test/testJsonMe.cpp index a6d3c6b54f..b19ef98d98 100644 --- a/modules/tracker/me/test/testJsonMe.cpp +++ b/modules/tracker/me/test/testJsonMe.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/tracker/me/test/testNurbs.cpp b/modules/tracker/me/test/testNurbs.cpp index 6fcbb26581..7390d7060a 100644 --- a/modules/tracker/me/test/testNurbs.cpp +++ b/modules/tracker/me/test/testNurbs.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/vision/test/homography/testDisplacement.cpp b/modules/vision/test/homography/testDisplacement.cpp index f0284e9511..cd5ac912c7 100644 --- a/modules/vision/test/homography/testDisplacement.cpp +++ b/modules/vision/test/homography/testDisplacement.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/vision/test/keypoint-with-dataset/testKeyPoint-2.cpp b/modules/vision/test/keypoint-with-dataset/testKeyPoint-2.cpp index 025a6a2dec..4c2d51524b 100644 --- a/modules/vision/test/keypoint-with-dataset/testKeyPoint-2.cpp +++ b/modules/vision/test/keypoint-with-dataset/testKeyPoint-2.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,6 +31,12 @@ * Test keypoint matching and pose estimation. */ +/*! + \example testKeyPoint-2.cpp + + \brief Test keypoint matching and pose estimation. + */ + #include #include @@ -425,11 +431,6 @@ void run_test(const std::string &env_ipath, bool opt_click_allowed, bool opt_dis } } -/*! - \example testKeyPoint-2.cpp - - \brief Test keypoint matching and pose estimation. -*/ int main(int argc, const char **argv) { try { diff --git a/modules/vision/test/keypoint-with-dataset/testKeyPoint-3.cpp b/modules/vision/test/keypoint-with-dataset/testKeyPoint-3.cpp index 37552fe411..f4c4d67076 100644 --- a/modules/vision/test/keypoint-with-dataset/testKeyPoint-3.cpp +++ b/modules/vision/test/keypoint-with-dataset/testKeyPoint-3.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -32,6 +32,13 @@ * to detect potential memory leaks in testKeyPoint.cpp. */ +/*! + \example testKeyPoint-3.cpp + + \brief Test keypoint matching with mostly OpenCV functions calls + to detect potential memory leaks in testKeyPoint.cpp. +*/ + #include #include @@ -264,12 +271,6 @@ void run_test(const std::string &env_ipath, bool opt_click_allowed, bool opt_dis } } -/*! - \example testKeyPoint-3.cpp - - \brief Test keypoint matching with mostly OpenCV functions calls - to detect potential memory leaks in testKeyPoint.cpp. -*/ int main(int argc, const char **argv) { try { diff --git a/modules/vision/test/keypoint-with-dataset/testKeyPoint-4.cpp b/modules/vision/test/keypoint-with-dataset/testKeyPoint-4.cpp index e310246933..eef653ded5 100644 --- a/modules/vision/test/keypoint-with-dataset/testKeyPoint-4.cpp +++ b/modules/vision/test/keypoint-with-dataset/testKeyPoint-4.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -32,6 +32,13 @@ * calls to detect potential memory leaks in testKeyPoint-2.cpp. */ +/*! + \example testKeyPoint-4.cpp + + \brief Test keypoint matching and pose estimation with mostly OpenCV + functions calls to detect potential memory leaks in testKeyPoint-2.cpp. +*/ + #include #include @@ -376,12 +383,6 @@ void run_test(const std::string &env_ipath, bool opt_click_allowed, bool opt_dis } } -/*! - \example testKeyPoint-4.cpp - - \brief Test keypoint matching and pose estimation with mostly OpenCV - functions calls to detect potential memory leaks in testKeyPoint-2.cpp. -*/ int main(int argc, const char **argv) { try { diff --git a/modules/vision/test/keypoint-with-dataset/testKeyPoint-5.cpp b/modules/vision/test/keypoint-with-dataset/testKeyPoint-5.cpp index cc874ebfd4..71912cb1a5 100644 --- a/modules/vision/test/keypoint-with-dataset/testKeyPoint-5.cpp +++ b/modules/vision/test/keypoint-with-dataset/testKeyPoint-5.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -32,6 +32,13 @@ * feature misssing in OpenCV 3.0. */ +/*! + \example testKeyPoint-5.cpp + + \brief Test keypoints detection with OpenCV, specially the Pyramid + implementation feature missing in OpenCV 3.0. +*/ + #include #include @@ -307,12 +314,6 @@ void run_test(const std::string &env_ipath, bool opt_click_allowed, bool opt_dis } } -/*! - \example testKeyPoint-5.cpp - - \brief Test keypoints detection with OpenCV, specially the Pyramid - implementation feature missing in OpenCV 3.0. -*/ int main(int argc, const char **argv) { try { diff --git a/modules/vision/test/keypoint-with-dataset/testKeyPoint-6.cpp b/modules/vision/test/keypoint-with-dataset/testKeyPoint-6.cpp index 0e5d86ace9..2b5471617a 100644 --- a/modules/vision/test/keypoint-with-dataset/testKeyPoint-6.cpp +++ b/modules/vision/test/keypoint-with-dataset/testKeyPoint-6.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,6 +31,12 @@ * Test descriptor computation. */ +/*! + \example testKeyPoint-6.cpp + + \brief Test descriptor extraction. +*/ + #include #include @@ -287,7 +293,7 @@ void run_test(const std::string &env_ipath, bool opt_click_allowed, bool opt_dis // Init BIN BOOST descriptor for FAST keypoints boostDesc = cv::xfeatures2d::BoostDesc::create(cv::xfeatures2d::BoostDesc::BINBOOST_256, true, 5.0f); #endif - } + } double t = vpTime::measureTimeMs(); cv::Mat descriptor; @@ -319,7 +325,7 @@ void run_test(const std::string &env_ipath, bool opt_click_allowed, bool opt_dis vpDisplay::getClick(I); } } - } + } std::cout << "\n\n"; @@ -366,7 +372,7 @@ void run_test(const std::string &env_ipath, bool opt_click_allowed, bool opt_dis // Init BIN BOOST descriptor for FAST keypoints boostDesc = cv::xfeatures2d::BoostDesc::create(cv::xfeatures2d::BoostDesc::BINBOOST_256, true, 5.0f); #endif - } + } double t = vpTime::measureTimeMs(); cv::Mat descriptor; @@ -400,14 +406,9 @@ void run_test(const std::string &env_ipath, bool opt_click_allowed, bool opt_dis vpDisplay::getClick(I); } } - } } +} - /*! - \example testKeyPoint-6.cpp - - \brief Test descriptor extraction. - */ int main(int argc, const char **argv) { try { diff --git a/modules/vision/test/keypoint-with-dataset/testKeyPoint-7.cpp b/modules/vision/test/keypoint-with-dataset/testKeyPoint-7.cpp index b526fe8090..2b04192bdc 100644 --- a/modules/vision/test/keypoint-with-dataset/testKeyPoint-7.cpp +++ b/modules/vision/test/keypoint-with-dataset/testKeyPoint-7.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,6 +31,12 @@ * Test saving / loading learning files for vpKeyPoint class. */ +/*! + \example testKeyPoint-7.cpp + + \brief Test saving / loading learning file. +*/ + #include #include @@ -637,11 +643,6 @@ template void run_test(const std::string &env_ipath, const std:: #endif } -/*! - \example testKeyPoint-7.cpp - - \brief Test saving / loading learning file. -*/ int main(int argc, const char **argv) { try { diff --git a/modules/vision/test/keypoint-with-dataset/testKeyPoint.cpp b/modules/vision/test/keypoint-with-dataset/testKeyPoint.cpp index 2c833c7347..5278fe739e 100644 --- a/modules/vision/test/keypoint-with-dataset/testKeyPoint.cpp +++ b/modules/vision/test/keypoint-with-dataset/testKeyPoint.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,6 +31,12 @@ * Test keypoint matching. */ +/*! + \example testKeyPoint.cpp + + \brief Test keypoint matching. +*/ + #include #include @@ -225,11 +231,6 @@ void run_test(const std::string &env_ipath, bool opt_click_allowed, bool opt_dis } } -/*! - \example testKeyPoint.cpp - - \brief Test keypoint matching. -*/ int main(int argc, const char **argv) { try { diff --git a/modules/vision/test/keypoint-with-dataset/testXmlConfigParserKeyPoint.cpp b/modules/vision/test/keypoint-with-dataset/testXmlConfigParserKeyPoint.cpp index 04bd9ca326..1612df36d9 100644 --- a/modules/vision/test/keypoint-with-dataset/testXmlConfigParserKeyPoint.cpp +++ b/modules/vision/test/keypoint-with-dataset/testXmlConfigParserKeyPoint.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/vision/test/pose-with-dataset/testPoseRansac2.cpp b/modules/vision/test/pose-with-dataset/testPoseRansac2.cpp index a332b544ed..65c8c2dae3 100644 --- a/modules/vision/test/pose-with-dataset/testPoseRansac2.cpp +++ b/modules/vision/test/pose-with-dataset/testPoseRansac2.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -31,6 +31,10 @@ * Test RANSAC 3D pose estimation method. */ +/*! + \example testPoseRansac2.cpp + */ + #include #ifdef VISP_HAVE_CATCH2 diff --git a/modules/vision/test/pose/testFindMatch.cpp b/modules/vision/test/pose/testFindMatch.cpp index 7431f3dca0..f73f646c2a 100644 --- a/modules/vision/test/pose/testFindMatch.cpp +++ b/modules/vision/test/pose/testFindMatch.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/vision/test/pose/testPose.cpp b/modules/vision/test/pose/testPose.cpp index 0d9f034acb..c5f584b7fe 100644 --- a/modules/vision/test/pose/testPose.cpp +++ b/modules/vision/test/pose/testPose.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/vision/test/pose/testPoseFeatures.cpp b/modules/vision/test/pose/testPoseFeatures.cpp index 8dbe26bc50..7502c94974 100644 --- a/modules/vision/test/pose/testPoseFeatures.cpp +++ b/modules/vision/test/pose/testPoseFeatures.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 diff --git a/modules/vision/test/pose/testPoseRansac.cpp b/modules/vision/test/pose/testPoseRansac.cpp index fd01d591a2..078d0715ec 100644 --- a/modules/vision/test/pose/testPoseRansac.cpp +++ b/modules/vision/test/pose/testPoseRansac.cpp @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -129,4 +129,4 @@ int main() std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl; return EXIT_SUCCESS; #endif - } +} diff --git a/modules/vs/test/visual-feature/testFeature.cpp b/modules/vs/test/visual-feature/testFeature.cpp index f355cdff11..f40891369b 100644 --- a/modules/vs/test/visual-feature/testFeature.cpp +++ b/modules/vs/test/visual-feature/testFeature.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,9 +29,7 @@ * * Description: * Visual feature manipulation. - * - * -*****************************************************************************/ + */ #include #include diff --git a/modules/vs/test/visual-feature/testFeatureMoment.cpp b/modules/vs/test/visual-feature/testFeatureMoment.cpp index 2260abe132..c7a36cd4c4 100644 --- a/modules/vs/test/visual-feature/testFeatureMoment.cpp +++ b/modules/vs/test/visual-feature/testFeatureMoment.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,12 @@ * * Description: * Example of visual servoing with moments using a polygon as object container - * -*****************************************************************************/ + */ + +/*! + \example testFeatureMoment.cpp + */ + #include #include #include diff --git a/modules/vs/test/visual-feature/testFeatureSegment.cpp b/modules/vs/test/visual-feature/testFeatureSegment.cpp index 820a0db26f..e666bd6c49 100644 --- a/modules/vs/test/visual-feature/testFeatureSegment.cpp +++ b/modules/vs/test/visual-feature/testFeatureSegment.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -30,8 +29,13 @@ * * Description: * Visual feature manipulation (segment). - * -*****************************************************************************/ + */ + +/*! + \example testFeatureSegment.cpp + + Shows how to build a task with a segment visual feature. +*/ #include #include @@ -58,13 +62,6 @@ #include #include //visual servoing task -/*! - - \example testFeatureSegment.cpp - - Shows how to build a task with a segment visual feature. - -*/ int main(int argc, const char **argv) { #ifdef ENABLE_VISP_NAMESPACE From 565ce61b0500b013863ed5a9e335263c553c2223 Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Thu, 20 Jun 2024 12:09:01 +0200 Subject: [PATCH 14/14] Improve doxygen doc --- ...ntour.dox => tutorial-imgproc-contour.dox} | 43 ++++++------- tutorial/image/drawingHelpers.cpp | 62 +++++++++---------- .../hough-transform/drawingHelpers.cpp | 22 +++---- 3 files changed, 60 insertions(+), 67 deletions(-) rename doc/tutorial/imgproc/{tutorial-imgproc-contour.doctutorial-imgproc-contour.dox => tutorial-imgproc-contour.dox} (76%) diff --git a/doc/tutorial/imgproc/tutorial-imgproc-contour.doctutorial-imgproc-contour.dox b/doc/tutorial/imgproc/tutorial-imgproc-contour.dox similarity index 76% rename from doc/tutorial/imgproc/tutorial-imgproc-contour.doctutorial-imgproc-contour.dox rename to doc/tutorial/imgproc/tutorial-imgproc-contour.dox index aeb8e8e536..8ef81cbba6 100644 --- a/doc/tutorial/imgproc/tutorial-imgproc-contour.doctutorial-imgproc-contour.dox +++ b/doc/tutorial/imgproc/tutorial-imgproc-contour.dox @@ -7,23 +7,23 @@ This tutorial will show you how to extract the contours from a binary image. The contour extraction algorithm is based on \cite articleSuzuki article and most of the implementation has been ported from \cite Hare:2011:OIJ:2072298.2072421 library. -The function to call is vp::findContours(const vpImage &, vpContour &, std::vector > &, const vpContourRetrievalType&): +The function to call is findContours(const vpImage &, vpContour &, std::vector > &, const vpContourRetrievalType &) - the first argument is the image where '0' pixel value means the background and '1' pixel value means the foreground. **Other values are not allowed.** -- the second argument is a vp::vpContour structure that contains the list of contours in a tree +- the second argument is a VISP_NAMESPACE_NAME::vpContour structure that contains the list of contours in a tree - the third argument is the list of contours -- the last argument is an option to choose the type of contour extraction, see vp::vpContourRetrievalType +- the last argument is an option to choose the type of contour extraction, see VISP_NAMESPACE_NAME::vpContourRetrievalType -The vp::vpContour structure is composed of: -- std::vector< \ref vp::vpContour * > m_children, the list of children contours for the current contour -- vp::vpContourType m_contourType, the type of contour (vp::CONTOUR_OUTER or vp::CONTOUR_HOLE) -- vp::vpContour * m_parent, the parent contour for the current contour +The vpContour structure is composed of: +- std::vector< VISP_NAMESPACE_NAME::vpContour * > m_children, the list of children contours for the current contour +- vpContourType m_contourType, the type of contour (VISP_NAMESPACE_NAME::CONTOUR_OUTER or VISP_NAMESPACE_NAME::CONTOUR_HOLE) +- VISP_NAMESPACE_NAME::vpContour * m_parent, the parent contour for the current contour - std::vector< \ref vpImagePoint > m_points, the list of contour points -- the first or top level contour is called the root contour (with vp::CONTOUR_HOLE type by default) and contains in \a m_children the list of contours +- the first or top level contour is called the root contour (with VISP_NAMESPACE_NAME::CONTOUR_HOLE type by default) and contains in \a m_children the list of contours The different contour extraction methods are: -- vp::CONTOUR_RETR_TREE, all the contours are extracted and stored in a hierarchical tree. -- vp::CONTOUR_RETR_LIST, all the contours are extracted and stored in a list. The top level contour contains in \a m_children the list of all the extracted contours. -- vp::CONTOUR_RETR_EXTERNAL, only the external contours are extracted and stored in a list. The top level contour contains in \a m_children the list of the external extracted contours. +- VISP_NAMESPACE_NAME::CONTOUR_RETR_TREE, all the contours are extracted and stored in a hierarchical tree. +- VISP_NAMESPACE_NAME::CONTOUR_RETR_LIST, all the contours are extracted and stored in a list. The top level contour contains in \a m_children the list of all the extracted contours. +- VISP_NAMESPACE_NAME::CONTOUR_RETR_EXTERNAL, only the external contours are extracted and stored in a list. The top level contour contains in \a m_children the list of the external extracted contours. The next section will provide a concrete example for better understanding. @@ -33,7 +33,7 @@ The following example also available in tutorial-contour.cpp will demonstrate on \include tutorial-contour.cpp -These functions are provided in a \a vp:: namespace and accessible using this include: +These functions are provided in a \a VISP_NAMESPACE_NAME namespace and accessible using this include: \snippet tutorial-contour.cpp Include @@ -44,21 +44,21 @@ The first steps are: \snippet tutorial-contour.cpp Otsu If the object of interest is in white in the image, the formula for the binarization is: \f[ - I_{bin}\left ( i,j \right ) = + I_{bin}\left ( i,j \right ) = \left \{ \begin{matrix} - 0 \text{ if } I_{src}\left ( i,j \right ) < \text{threshold} \\ + 0 \text{ if } I_{src}\left ( i,j \right ) < \text{threshold} \\ 1 \text{ otherwise} \end{matrix} \right. \f] If the object of interest is in black in the image, the formula for the binarization is: \f[ - I_{bin}\left ( i,j \right ) = + I_{bin}\left ( i,j \right ) = \left \{ \begin{matrix} - 1 \text{ if } I_{src}\left ( i,j \right ) < \text{threshold} \\ + 1 \text{ if } I_{src}\left ( i,j \right ) < \text{threshold} \\ 0 \text{ otherwise} \end{matrix} \right. \f] -- extract the contours (by default, it is the vp::CONTOUR_RETR_TREE method) +- extract the contours (by default, it is the VISP_NAMESPACE_NAME::CONTOUR_RETR_TREE method) \snippet tutorial-contour.cpp Find contours - draw the contours if wanted \snippet tutorial-contour.cpp Draw contours @@ -82,7 +82,8 @@ The image after binarisation: \image html img-tutorial-contour-binarisation2.png "Image after binarization using the Otsu method" -Instead of drawing all the contours with the same color, we can assign a first color for vp::CONTOUR_OUTER contour and a second color for vp::CONTOUR_HOLE contour. +Instead of drawing all the contours with the same color, we can assign a first color for +VISP_NAMESPACE_NAME::CONTOUR_OUTER contour and a second color for VISP_NAMESPACE_NAME::CONTOUR_HOLE contour. The function to navigate in the contour tree is the following: @@ -100,7 +101,7 @@ To display the hierarchy, we can use this function: \snippet tutorial-contour.cpp Print contours hierarchy func -For the vp::CONTOUR_RETR_TREE method, the output is: +For the VISP_NAMESPACE_NAME::CONTOUR_RETR_TREE method, the output is:
Contour:\n @@ -166,7 +167,7 @@ Contour:\n The top level contour is always the root contour with zero contour point and which contains the list of contours. -For the vp::CONTOUR_RETR_EXTERNAL method, the output is: +For the VISP_NAMESPACE_NAME::CONTOUR_RETR_EXTERNAL method, the output is:
Contour:\n @@ -198,7 +199,7 @@ The result image is: \image html img-tutorial-contour-draw-contours3.png "External contours extracted and displayed on a new image" -For the vp::CONTOUR_RETR_LIST method, the output is: +For the VISP_NAMESPACE_NAME::CONTOUR_RETR_LIST method, the output is:
Contour:\n diff --git a/tutorial/image/drawingHelpers.cpp b/tutorial/image/drawingHelpers.cpp index 2737a6cd9c..dd9e2630d0 100644 --- a/tutorial/image/drawingHelpers.cpp +++ b/tutorial/image/drawingHelpers.cpp @@ -32,44 +32,40 @@ #include -#ifdef ENABLE_VISP_NAMESPACE -using namespace VISP_NAMESPACE_NAME; -#endif - #if defined(VISP_HAVE_X11) -vpDisplayX drawingHelpers::d_Iinput; -vpDisplayX drawingHelpers::d_dIx; -vpDisplayX drawingHelpers::d_dIy; -vpDisplayX drawingHelpers::d_IcannyVisp; -vpDisplayX drawingHelpers::d_IcannyImgFilter; +VISP_NAMESPACE_ADDRESSING vpDisplayX drawingHelpers::d_Iinput; +VISP_NAMESPACE_ADDRESSING vpDisplayX drawingHelpers::d_dIx; +VISP_NAMESPACE_ADDRESSING vpDisplayX drawingHelpers::d_dIy; +VISP_NAMESPACE_ADDRESSING vpDisplayX drawingHelpers::d_IcannyVisp; +VISP_NAMESPACE_ADDRESSING vpDisplayX drawingHelpers::d_IcannyImgFilter; #elif defined(HAVE_OPENCV_HIGHGUI) -vpDisplayOpenCV drawingHelpers::d_Iinput; -vpDisplayOpenCV drawingHelpers::d_dIx; -vpDisplayOpenCV drawingHelpers::d_dIy; -vpDisplayOpenCV drawingHelpers::d_IcannyVisp; -vpDisplayOpenCV drawingHelpers::d_IcannyImgFilter; +VISP_NAMESPACE_ADDRESSING vpDisplayOpenCV drawingHelpers::d_Iinput; +VISP_NAMESPACE_ADDRESSING vpDisplayOpenCV drawingHelpers::d_dIx; +VISP_NAMESPACE_ADDRESSING vpDisplayOpenCV drawingHelpers::d_dIy; +VISP_NAMESPACE_ADDRESSING vpDisplayOpenCV drawingHelpers::d_IcannyVisp; +VISP_NAMESPACE_ADDRESSING vpDisplayOpenCV drawingHelpers::d_IcannyImgFilter; #elif defined(VISP_HAVE_GTK) -vpDisplayGTK drawingHelpers::d_Iinput; -vpDisplayGTK drawingHelpers::d_dIx; -vpDisplayGTK drawingHelpers::d_dIy; -vpDisplayGTK drawingHelpers::d_IcannyVisp; -vpDisplayGTK drawingHelpers::d_IcannyImgFilter; +VISP_NAMESPACE_ADDRESSING VISP_NAMESPACE_ADDRESSING vpDisplayGTK drawingHelpers::d_Iinput; +VISP_NAMESPACE_ADDRESSING vpDisplayGTK drawingHelpers::d_dIx; +VISP_NAMESPACE_ADDRESSING vpDisplayGTK drawingHelpers::d_dIy; +VISP_NAMESPACE_ADDRESSING vpDisplayGTK drawingHelpers::d_IcannyVisp; +VISP_NAMESPACE_ADDRESSING vpDisplayGTK drawingHelpers::d_IcannyImgFilter; #elif defined(VISP_HAVE_GDI) -vpDisplayGDI drawingHelpers::d_Iinput; -vpDisplayGDI drawingHelpers::d_dIx; -vpDisplayGDI drawingHelpers::d_dIy; -vpDisplayGDI drawingHelpers::d_IcannyVisp; -vpDisplayGDI drawingHelpers::d_IcannyImgFilter; +VISP_NAMESPACE_ADDRESSING vpDisplayGDI drawingHelpers::d_Iinput; +VISP_NAMESPACE_ADDRESSING vpDisplayGDI drawingHelpers::d_dIx; +VISP_NAMESPACE_ADDRESSING vpDisplayGDI drawingHelpers::d_dIy; +VISP_NAMESPACE_ADDRESSING vpDisplayGDI drawingHelpers::d_IcannyVisp; +VISP_NAMESPACE_ADDRESSING vpDisplayGDI drawingHelpers::d_IcannyImgFilter; #elif defined(VISP_HAVE_D3D9) -vpDisplayD3D drawingHelpers::d_Iinput; -vpDisplayD3D drawingHelpers::d_dIx; -vpDisplayD3D drawingHelpers::d_dIy; -vpDisplayD3D drawingHelpers::d_IcannyVisp; -vpDisplayD3D drawingHelpers::d_IcannyImgFilter; +VISP_NAMESPACE_ADDRESSING vpDisplayD3D drawingHelpers::d_Iinput; +VISP_NAMESPACE_ADDRESSING vpDisplayD3D drawingHelpers::d_dIx; +VISP_NAMESPACE_ADDRESSING vpDisplayD3D drawingHelpers::d_dIy; +VISP_NAMESPACE_ADDRESSING vpDisplayD3D drawingHelpers::d_IcannyVisp; +VISP_NAMESPACE_ADDRESSING vpDisplayD3D drawingHelpers::d_IcannyImgFilter; #endif -void drawingHelpers::init(vpImage &Iinput, vpImage &IcannyVisp, vpImage *p_dIx, - vpImage *p_dIy, vpImage *p_IcannyimgFilter) +void drawingHelpers::init(VISP_NAMESPACE_ADDRESSING vpImage &Iinput, VISP_NAMESPACE_ADDRESSING vpImage &IcannyVisp, VISP_NAMESPACE_ADDRESSING vpImage *p_dIx, + VISP_NAMESPACE_ADDRESSING vpImage *p_dIy, VISP_NAMESPACE_ADDRESSING vpImage *p_IcannyimgFilter) { #if defined(VISP_HAVE_DISPLAY) d_Iinput.init(Iinput, 10, 10); @@ -92,14 +88,14 @@ void drawingHelpers::init(vpImage &Iinput, vpImage #endif } -void drawingHelpers::display(vpImage &I, const std::string &title) +void drawingHelpers::display(VISP_NAMESPACE_ADDRESSING vpImage &I, const std::string &title) { vpDisplay::display(I); vpDisplay::setTitle(I, title); vpDisplay::flush(I); } -bool drawingHelpers::waitForClick(const vpImage &I, const bool &blockingMode) +bool drawingHelpers::waitForClick(VISP_NAMESPACE_ADDRESSING const vpImage &I, const bool &blockingMode) { vpDisplay::displayText(I, 15, 15, "Left click to continue...", vpColor::red); vpDisplay::displayText(I, 35, 15, "Right click to stop...", vpColor::red); diff --git a/tutorial/imgproc/hough-transform/drawingHelpers.cpp b/tutorial/imgproc/hough-transform/drawingHelpers.cpp index 69cad2121f..1c1d15db75 100644 --- a/tutorial/imgproc/hough-transform/drawingHelpers.cpp +++ b/tutorial/imgproc/hough-transform/drawingHelpers.cpp @@ -2,25 +2,21 @@ #include -#ifdef ENABLE_VISP_NAMESPACE -using namespace VISP_NAMESPACE_NAME; -#endif - #if defined(VISP_HAVE_X11) -vpDisplayX drawingHelpers::d; +VISP_NAMESPACE_ADDRESSING vpDisplayX drawingHelpers::d; #elif defined(VISP_HAVE_OPENCV) -vpDisplayOpenCV drawingHelpers::d; +VISP_NAMESPACE_ADDRESSING vpDisplayOpenCV drawingHelpers::d; #elif defined(VISP_HAVE_GTK) -vpDisplayGTK drawingHelpers::d; +VISP_NAMESPACE_ADDRESSING vpDisplayGTK drawingHelpers::d; #elif defined(VISP_HAVE_GDI) -vpDisplayGDI drawingHelpers::d; +VISP_NAMESPACE_ADDRESSING vpDisplayGDI drawingHelpers::d; #elif defined(VISP_HAVE_D3D9) -vpDisplayD3D drawingHelpers::d; +VISP_NAMESPACE_ADDRESSING vpDisplayD3D drawingHelpers::d; #endif -vpImage drawingHelpers::I_disp; +VISP_NAMESPACE_ADDRESSING vpImage drawingHelpers::I_disp; -bool drawingHelpers::display(vpImage &I, const std::string &title, const bool &blockingMode) +bool drawingHelpers::display(VISP_NAMESPACE_ADDRESSING vpImage &I, const std::string &title, const bool &blockingMode) { I_disp = I; #if defined(VISP_HAVE_DISPLAY) @@ -47,14 +43,14 @@ bool drawingHelpers::display(vpImage &I, const std::string &title, const return hasToContinue; } -bool drawingHelpers::display(vpImage &D, const std::string &title, const bool &blockingMode) +bool drawingHelpers::display(VISP_NAMESPACE_ADDRESSING vpImage &D, const std::string &title, const bool &blockingMode) { vpImage I; // Image to display vpImageConvert::convert(D, I); return display(I, title, blockingMode); } -bool drawingHelpers::display(vpImage &D, const std::string &title, const bool &blockingMode) +bool drawingHelpers::display(VISP_NAMESPACE_ADDRESSING vpImage &D, const std::string &title, const bool &blockingMode) { vpImage I; // Image to display vpImageConvert::convert(D, I);