diff --git a/.vscode/settings.json b/.vscode/settings.json index 1a735c4e79..2a31aeaf5f 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -16,7 +16,7 @@ "files.associations": { "bitset": "cpp", "optional": "cpp", - "*.in": "cpp", + "*.in": "cmake", "cctype": "cpp", "clocale": "cpp", "cmath": "cpp", @@ -109,7 +109,12 @@ "__string": "cpp", "compare": "cpp", "concepts": "cpp", - "*.ipp": "cpp" + "*.ipp": "cpp", + "any": "cpp", + "numbers": "cpp", + "semaphore": "cpp", + "stop_token": "cpp", + "view": "cpp" }, "C_Cpp.vcFormat.indent.namespaceContents": false, "editor.formatOnSave": true, diff --git a/ChangeLog.txt b/ChangeLog.txt index 88b1bede12..2a6ce1e923 100644 --- a/ChangeLog.txt +++ b/ChangeLog.txt @@ -825,7 +825,7 @@ ViSP 2.9.0 (released February 18th, 2014) . [16919] Unable to link with OpenCV 2.4.8 under Windows . [16924] MinGW: unable to build ViSP as a shared library . [16928] vpVideoReader is unable to find the number of the first image - from a generic sequence of images + from a generic sequence of images ---------------------------------------------- ViSP 2.8.0 (released July 24th, 2013) - New features @@ -975,21 +975,21 @@ ViSP 2.6.2 (released July 15, 2012) - Bug fixed - [13536] Division by zero in vpMeLine - [13548] ViSP-2.6.1 produces several warnings with g++ 4.4.3, a const - is missing in vpMbtDistanceLine.h + is missing in vpMbtDistanceLine.h - [13575] ViSP sporadic segmentation fault in vpMeLine - [13625] trackMeNurbs example produce an assertion failed with MSVC: "list iterator not incrementable" - [13626] testXmlParser produre a Debug assertion failed with MSVC - [13678] Cannot build all the examples if no display capability is - available + available - [13679] GDI not detected on Windows 7 (64 bit) with Visual C++ Express - 2010 + 2010 - [13689] Memory leaks in jpeg reader: vpImageIo::readJPEG() - [13690] Memory leaks in NURBS tracker - [13843] In vpPolygon, isInside() may produce a segfault and getArea() or getCenter() give bad results - [13850] visp-config.bat produces a non compatible msvc 2010 output - format + format - [13955] vpBasicFeature::error() is not virtual - [13963] Under Windows, vpDisplay::getClickUp() doesn't work - [14081] Error in vpThetaUVector when angles are near 180 deg +/- epsilon @@ -1123,7 +1123,7 @@ ViSP 2.6.0 (released September 23rd, 2010) - Documentation improvements - Deprecated . Some deprecated classes were removed: vpItifg8Grabber, - vpEulerVector, vpSubPixel, vpRA + vpEulerVector, vpSubPixel, vpRA . Deprecated functions since ViSP 2.4.4 where also removed - Examples: - New example calibrateTsai.cpp to show how to use the vpCalibration @@ -1144,7 +1144,7 @@ ViSP 2.6.0 (released September 23rd, 2010) - [10748] PThread and Win32 Conflict - [10928] vpPlane: rayIntersection from 2 points - [10985] In vpPlot, 2D curve plotting starts with point (0,0) - and not with the first specified point. + and not with the first specified point. ---------------------------------------------- ViSP 2.4.4 (released January 8th, 2010) @@ -1210,23 +1210,23 @@ ViSP 2.4.4-rc1 (released December 21th, 2009) - Bug fixed - [7638] Memory leak in all vpFeature... classes was reopened - [7888] vpV4l2Grabber cannot acquire an image from an analog camera - on Ubuntu 8.04 Hardy Heron + on Ubuntu 8.04 Hardy Heron - [7994] vpMeLine : initialization problem for horizontal lines - [8001] Coin and SoQt are not detected during CMake configuration - under Fedora 10 + under Fedora 10 - [8032] Link error with GSL while compiling HelloWorld.cpp on OSX - [8225] Bug in the multiplication of a twist matrix (6x6) by a matrix (6xn) - [8248] Compilation error under Windows MSCV8 if deprecated functions - are not build + are not build - [8249] Compilation error under Windows MSCV8 when building ViSP - as a shared library (DLL) + as a shared library (DLL) - [8257] Bug in vp1394TwoGrabber::resetBus() - [8259] Segfault in vpDisplayX - [8389] vpKltOpencv : Bug in the copy constructor - [8493] Problem to build ViSP as a dynamic library under windows - (Visual C++) + (Visual C++) - [8495] Segmentation fault during the execution of the example - grabDirectShow + grabDirectShow - [8831] Possible bug when extracting axis/angle from theta/u vector ---------------------------------------------- ViSP 2.4.3 (released April 10th, 2009) @@ -1243,7 +1243,7 @@ ViSP 2.4.3 (released April 10th, 2009) - [7494] Division by 0 in vpHomography - [7424] Coin3D version 3 not supported under Windows - [7613] vpSimulator::getInternalImage() function yields to an infinite - loop. + loop. - [7607] vpDisplayGTK bad window positioning - [7638] Memory leak in all vpFeature... classes @@ -1370,15 +1370,15 @@ ViSP 2.4.2 (released March 27th, 2008) vpServoException::notKilledProperly is launched by vpServo::~vpServo() destructor. - vpMatrix : Change Infinity Norm and Euclidean Norm formula to match with - the standard matrix norms definitions. + the standard matrix norms definitions. Change determinant formula for 3x3 matrices to match with the standard determinant definition. - vpImageConvert : - Add IplImage to vpImage and vpImage to IplImage conversion tool. - Add MONO16 to grey (MONO8) conversion tool. + Add IplImage to vpImage and vpImage to IplImage conversion tool. + Add MONO16 to grey (MONO8) conversion tool. - vp1394TwoGrabber : - Supports libdc1394-2.0.0-rc7 and libdc1394-2.0.1 - MONO16 color coding is supported + Supports libdc1394-2.0.0-rc7 and libdc1394-2.0.1 + MONO16 color coding is supported - Testing and examples: - example/calibration/calibrate2dGrid.cpp was added - example/tracking/trackKltOpencv.cpp was added @@ -1399,104 +1399,104 @@ ViSP 2.4.1 (released May 16th, 2007) - New classes - vpRect class for rectangle manipulations, display - vpHistogram,vpHistogramPeak and vpHistogramValey classes to - handle gray level image histogram - - vpSubPixel for sub-pixel manipulations - - vpMouseButton for mouse click identification + handle gray level image histogram + - vpSubPixel for sub-pixel manipulations + - vpMouseButton for mouse click identification - New Features - MinGW supported - - ViSP as a third party library in an other project. - It is now possible to use ViSP as a third party project without - installing ViSP by make install. ViSP can now be used - directly from the build dir, or from the install dir, or - from a package (NSIS, ...) - - vpRobotCamera It is now possible to add a sampling time so that - the behavior of this simulated camera is quite realistic. This - sampling time is set by default to 40 ms. - - vpExponentialMap A sampling time was introduced. + - ViSP as a third party library in an other project. + It is now possible to use ViSP as a third party project without + installing ViSP by make install. ViSP can now be used + directly from the build dir, or from the install dir, or + from a package (NSIS, ...) + - vpRobotCamera It is now possible to add a sampling time so that + the behavior of this simulated camera is quite realistic. This + sampling time is set by default to 40 ms. + - vpExponentialMap A sampling time was introduced. - Bug fix and improvement - - vpImage: getWidth() and getHeight() replace getCols() and getRows() - which are now obsolete. - - More restrictive types in vpImage, vpDisplay..., and vp...Grabber - classes: const are added, unsigned int replace int for pixel - manipulations and image or display size. - - In vpDisplay classes: - . vpMouseButton::vpMouseButtonType was introduced to handle - mouse clicks - . vpColor::vpColorType was introduced to handle colors to display - - Simulator functionality based on Coin usage is now available - under windows. It allows the use of vpSimulator class under this - platform. - - Compatibility with Windows platform. - - Reference documentation improvement - - cfox and std namespace were removed. To use cout you need to call - std::cout, std::endl, std::cin ... + - vpImage: getWidth() and getHeight() replace getCols() and getRows() + which are now obsolete. + - More restrictive types in vpImage, vpDisplay..., and vp...Grabber + classes: const are added, unsigned int replace int for pixel + manipulations and image or display size. + - In vpDisplay classes: + . vpMouseButton::vpMouseButtonType was introduced to handle + mouse clicks + . vpColor::vpColorType was introduced to handle colors to display + - Simulator functionality based on Coin usage is now available + under windows. It allows the use of vpSimulator class under this + platform. + - Compatibility with Windows platform. + - Reference documentation improvement + - cfox and std namespace were removed. To use cout you need to call + std::cout, std::endl, std::cin ... ---------------------------------------------- ViSP 2.4.0 (released February 6th, 2007) - New Feature - - add new framegrabber interface for itifg8 driver from Coreco - Imaging Technology (http://sourceforge.net/projects/itifg) - Tested with AM-STD COMP framegrabber board on 2.4 and 2.6 - linux kernel (see vpItifg8Grabber). - - add new framegrabber interface and test for firewire 1394 - cameras using libdc1394-2.x. Need a newer version than - libdc1394-2.0.0-rc4. - Tested with the svn version of libdc1394-2 and Marlin F033C and - F131B cameras (see vp1394TwoGrabber). Format 7 is supported - in vp1394TwoGrabber. - vp1394Grabber will be obsolete soon. + - add new framegrabber interface for itifg8 driver from Coreco + Imaging Technology (http://sourceforge.net/projects/itifg) + Tested with AM-STD COMP framegrabber board on 2.4 and 2.6 + linux kernel (see vpItifg8Grabber). + - add new framegrabber interface and test for firewire 1394 + cameras using libdc1394-2.x. Need a newer version than + libdc1394-2.0.0-rc4. + Tested with the svn version of libdc1394-2 and Marlin F033C and + F131B cameras (see vp1394TwoGrabber). Format 7 is supported + in vp1394TwoGrabber. + vp1394Grabber will be obsolete soon. - Bug fix and improvement - - tests and examples - - Video For Linux Two grabber (see vpV4l2Grabber) - - memory leaks suppression - - works fully under windows - - doxygen documentation + - tests and examples + - Video For Linux Two grabber (see vpV4l2Grabber) + - memory leaks suppression + - works fully under windows + - doxygen documentation ---------------------------------------------- ViSP 2.2.0 (released August 25th, 2006) - Build - - To increase the code portability, especially under windows), - all the build process is now based on CMake - (see http://www.cmake.org). - - Supported platforms: - Linux, Mac OSX, SunOS: g++ 2.95.3, 2.96, 3.3.x, 3.4.x, 4.1.0) - Windows: MSVC7 (2003.NET), MSVC8 (2005) - - Suppression of autotools support (configure) - - Allows shared library building (.so and .dll) + - To increase the code portability, especially under windows), + all the build process is now based on CMake + (see http://www.cmake.org). + - Supported platforms: + Linux, Mac OSX, SunOS: g++ 2.95.3, 2.96, 3.3.x, 3.4.x, 4.1.0) + Windows: MSVC7 (2003.NET), MSVC8 (2005) + - Suppression of autotools support (configure) + - Allows shared library building (.so and .dll) - New Feature - - add control for Traclabs biclops pan-tilt head - - add control for Directed Perception ptu-46 pan-tilt head - - add vpDisplayGTK display under Windows - - add vpDirectShowGrabber framegrabber under Windows - - add vpDisplayGDI display under windows + - add control for Traclabs biclops pan-tilt head + - add control for Directed Perception ptu-46 pan-tilt head + - add vpDisplayGTK display under Windows + - add vpDirectShowGrabber framegrabber under Windows + - add vpDisplayGDI display under windows - Bug fix and improvement - - vp1394Grabber settings (format, mode, framerate, shutter, gain) - - vpDot (Moment computation) - - some fixes in vpDisplayGTK + - vp1394Grabber settings (format, mode, framerate, shutter, gain) + - vpDot (Moment computation) + - some fixes in vpDisplayGTK ---------------------------------------------- ViSP 2.1.1 (released January 6th, 2006) - - New examples - - create an example directory - - add vpDotExample.cpp - - add vpDot2Example.cpp - - add vpPoseExample.cpp - - add 2D0.5.VisualServoingSimulation.cpp + - New examples + - create an example directory + - add vpDotExample.cpp + - add vpDot2Example.cpp + - add vpPoseExample.cpp + - add 2D0.5.VisualServoingSimulation.cpp - New Feature - - add this ChangleLog file - - add a vpDisplayGTK class with the GTK2+ window system - - new member function in vpDisplay (_uv) - - add function getInteractionMatrix() in vpGenericFeature + - add this ChangleLog file + - add a vpDisplayGTK class with the GTK2+ window system + - new member function in vpDisplay (_uv) + - add function getInteractionMatrix() in vpGenericFeature - Specification modifications - - change the vpDot and vpDot2 specifications - - supress demo directory + - change the vpDot and vpDot2 specifications + - supress demo directory - Bug fix and improvement - - add a runtime warning when vpGenericFeature is not correctly used - - comment in english in vpDisplayX - - suppress a trace in vpPoseVirtualVisualServoing.cpp - - directory.mk : add -p option to mkdir - - improve XP compatibility - - in vp1394Grabber, latency and memory leaks suppression + - add a runtime warning when vpGenericFeature is not correctly used + - comment in english in vpDisplayX + - suppress a trace in vpPoseVirtualVisualServoing.cpp + - directory.mk : add -p option to mkdir + - improve XP compatibility + - in vp1394Grabber, latency and memory leaks suppression - WWW - add an image sequence to download on the www site cube.tar.gz diff --git a/LICENSE.txt b/LICENSE.txt index 7dc01a39fb..06f4de5e7d 100644 --- a/LICENSE.txt +++ b/LICENSE.txt @@ -1,4 +1,4 @@ - GNU GENERAL PUBLIC LICENSE + GNU GENERAL PUBLIC LICENSE Version 2 or (at your option) any later version, June 1991 Copyright (C) 1989, 1991 Free Software Foundation, Inc. @@ -6,7 +6,7 @@ Everyone is permitted to copy and distribute verbatim copies of this license document, but changing it is not allowed. - Preamble + Preamble The licenses for most software are designed to take away your freedom to share and change it. By contrast, the GNU General Public @@ -56,7 +56,7 @@ patent must be licensed for everyone's free use or not licensed at all. The precise terms and conditions for copying, distribution and modification follow. - GNU GENERAL PUBLIC LICENSE + GNU GENERAL PUBLIC LICENSE TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION 0. This License applies to any program or other work which contains @@ -255,7 +255,7 @@ make exceptions for this. Our decision will be guided by the two goals of preserving the free status of all derivatives of our free software and of promoting the sharing and reuse of software generally. - NO WARRANTY + NO WARRANTY 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN @@ -277,9 +277,9 @@ YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGES. - END OF TERMS AND CONDITIONS + END OF TERMS AND CONDITIONS - How to Apply These Terms to Your New Programs + How to Apply These Terms to Your New Programs If you develop a new program, and you want it to be of the greatest possible use to the public, the best way to achieve this is to make it diff --git a/cmake/VISPGenerateConfigScript.cmake b/cmake/VISPGenerateConfigScript.cmake index 45f33e7285..c55a219a4c 100644 --- a/cmake/VISPGenerateConfigScript.cmake +++ b/cmake/VISPGenerateConfigScript.cmake @@ -67,7 +67,7 @@ macro(fix_prefix lst isown) list(APPEND _lst "${item}") elseif(item MATCHES "^-framework") # MacOS framework (assume single entry "-framework OpenCL") list(APPEND _lst "${item}") - elseif(item MATCHES "^-") #could be "-pthread" (occured with Ubuntu 18.04) + elseif(item MATCHES "^-") #could be "-pthread" (occurred with Ubuntu 18.04) list(APPEND _lst "${item}") elseif(item MATCHES ".framework/" OR item MATCHES "/([^/]+)\\.framework$") vp_get_framework(_fmk_name "${item}" NAME) @@ -180,6 +180,8 @@ if(NOT DEFINED CMAKE_HELPER_SCRIPT) VISP_RUNTIME VISP_HAVE_OPENMP + WITH_CATCH2 + FILE_VISP_SCRIPT_CONFIG FILE_VISP_SCRIPT_CONFIG_INSTALL FILE_VISP_SCRIPT_PC_INSTALL @@ -272,11 +274,15 @@ else() # DEFINED CMAKE_HELPER_SCRIPT "${_includes_modules}" "${_includes_extra}") + if(WITH_CATCH2) + list(APPEND VISP_SONARQUBE_INCLUDE_DIRS "${VISP_SOURCE_DIR}/3rdparty/catch2") + endif() + vp_list_replace_separator(VISP_SONARQUBE_INCLUDE_DIRS ", ") configure_file("${VISP_SOURCE_DIR}/cmake/templates/sonar-project.properties.in" "${VISP_BINARY_DIR}/sonar-project.properties" - @ONLY ) + @ONLY) ####################################################################### # diff --git a/cmake/templates/sonar-project.properties.in b/cmake/templates/sonar-project.properties.in index 63b33bfb59..f2881e205d 100644 --- a/cmake/templates/sonar-project.properties.in +++ b/cmake/templates/sonar-project.properties.in @@ -7,15 +7,14 @@ sonar.projectKey=rainbow:visp:master # Project description sonar.projectName=rainbow:visp:master # Used to compare metrics with v1.10 for example -sonar.projectVersion=3.2 +sonar.projectVersion=@VISP_VERSION@ # Path is relative to the sonar-project.properties file. Replace "\" by "/" on Windows. sonar.sources=@VISP_SOURCE_DIR@ sonar.projectBaseDir=@VISP_SOURCE_DIR@ -sonar.exclusions=3rdparty/**/*,cmake/**/*,platforms/**/*,doc/**/*,modules/**/*.txt,modules/**/*.py,modules/java/**/*,tutorial/java/**/*,**/arms/*,**/scene/* +sonar.exclusions=codecov.yml,appveyor.yml,3rdparty/**/*,cmake/**/*,doc/**/*,modules/**/*.txt,modules/**/*.py,modules/core/src/image/private/stb_truetype.h,platforms/**/*,script/**/*,**/arms/*,**/scene/*,**/*.pb,**/*.pbtxt,**/*.skeleton,**/*.exr,**/*.FCStd,**/*.blend,**/*.mesh,**/*.pfm,**/*.sb2,**/*.java,ci/**/* # Source code language sonar.language=c++ sonar.cxx.includeDirectories=@VISP_SONARQUBE_INCLUDE_DIRS@ -# Empty c lang patterns to avoid error: -# "Language of file '*.h' can not be decided as the file matches patterns of both -# sonar.lang.patterns.c++ and sonar.lang.patterns.c" -sonar.lang.patterns.c="" +sonar.cxx.file.suffixes=.cxx,.cpp,.cc,.c,.hxx,.hpp,.hh,.h +sonar.cxx.defines=VISP_EXPORT \n\ + DOXYGEN_SHOULD_SKIP_THIS \n\ diff --git a/doc/tutorial/visual-servo/tutorial-franka-pbvs.dox b/doc/tutorial/visual-servo/tutorial-franka-pbvs.dox index f8d080a448..9d3a4c6e4f 100644 --- a/doc/tutorial/visual-servo/tutorial-franka-pbvs.dox +++ b/doc/tutorial/visual-servo/tutorial-franka-pbvs.dox @@ -309,7 +309,7 @@ Franka network exception: libfranka: Connection to FCI refused. Please install F Check if you are connected to the Franka robot or if you specified the right IP using --ip command line option set by default to 192.168.1.1. \endverbatim -This error occured with our robot after upgrading the Franka system from 4.0.2 to 4.2.2 version. +This error occurred with our robot after upgrading the Franka system from 4.0.2 to 4.2.2 version. \image html img-franka-system-version.jpg The Dashboard shows that after a synchonization using Franka World, our System version is 4.2.2. diff --git a/doc/tutorial/visual-servo/tutorial-pixhawk-vs.dox b/doc/tutorial/visual-servo/tutorial-pixhawk-vs.dox index 50b9a292c4..a34dd17df7 100644 --- a/doc/tutorial/visual-servo/tutorial-pixhawk-vs.dox +++ b/doc/tutorial/visual-servo/tutorial-pixhawk-vs.dox @@ -97,7 +97,7 @@ it will enable vpRobotMavlink usage. When using PX4, it is important to install [QGroundControl](http://qgroundcontrol.com/) (QGC) following instructions [here](https://docs.qgroundcontrol.com/master/en/getting_started/download_and_install.html). - \note When launching `QGroundControl` if you experience the following error (occured with Ubuntu 22.04): + \note When launching `QGroundControl` if you experience the following error (occurred with Ubuntu 22.04): \verbatim $ ./QGroundControl.AppImage dlopen(): error loading libfuse.so.2 diff --git a/example/device/display/displayD3D.cpp b/example/device/display/displayD3D.cpp index 987d01b79c..934b2cf5ae 100644 --- a/example/device/display/displayD3D.cpp +++ b/example/device/display/displayD3D.cpp @@ -88,7 +88,7 @@ the image and the overlayed features in an image on the disk\n\ \n\ SYNOPSIS\n\ %s [-i ] [-o ]\n\ - [-c] [-d] [-h]\n \ + [-c] [-d] [-h]\n \ ", name); diff --git a/example/device/display/displayGDI.cpp b/example/device/display/displayGDI.cpp index fcfa04e8b3..17bebbd21f 100644 --- a/example/device/display/displayGDI.cpp +++ b/example/device/display/displayGDI.cpp @@ -89,7 +89,7 @@ the image and the overlayed features in an image on the disk\n\ \n\ SYNOPSIS\n\ %s [-i ] [-o ]\n\ - [-c] [-d] [-h]\n \ + [-c] [-d] [-h]\n \ ", name); diff --git a/example/device/display/displayGTK.cpp b/example/device/display/displayGTK.cpp index 7cbcb9132c..a80883c498 100644 --- a/example/device/display/displayGTK.cpp +++ b/example/device/display/displayGTK.cpp @@ -90,7 +90,7 @@ the image and the overlayed features in an image on the disk\n\ \n\ SYNOPSIS\n\ %s [-i ] [-o ]\n\ - [-c] [-d] [-h]\n \ + [-c] [-d] [-h]\n \ ", name); diff --git a/example/device/display/displayOpenCV.cpp b/example/device/display/displayOpenCV.cpp index 83c37f289b..a0968ada52 100644 --- a/example/device/display/displayOpenCV.cpp +++ b/example/device/display/displayOpenCV.cpp @@ -88,7 +88,7 @@ the image and the overlayed features in an image on the disk.\n\ \n\ SYNOPSIS\n\ %s [-i ] [-o ]\n\ - [-c] [-d] [-h]\n \ + [-c] [-d] [-h]\n \ ", name); diff --git a/example/device/display/displayX.cpp b/example/device/display/displayX.cpp index 5ccf902cac..595c794d3a 100644 --- a/example/device/display/displayX.cpp +++ b/example/device/display/displayX.cpp @@ -86,7 +86,7 @@ the image and the overlayed features in an image on the disk.\n\ \n\ SYNOPSIS\n\ %s [-i ] [-o ]\n\ - [-c] [-d] [-h]\n \ + [-c] [-d] [-h]\n \ ", name); diff --git a/example/device/display/displayXMulti.cpp b/example/device/display/displayXMulti.cpp index 2df1e2fe37..0981c574f6 100644 --- a/example/device/display/displayXMulti.cpp +++ b/example/device/display/displayXMulti.cpp @@ -89,7 +89,7 @@ the image and the overlayed features in an image on the disk.\n\ \n\ SYNOPSIS\n\ %s [-i ] [-o ]\n\ - [-c] [-d] [-h]\n \ + [-c] [-d] [-h]\n\ ", name); diff --git a/example/device/framegrabber/grabDirectShowMulti.cpp b/example/device/framegrabber/grabDirectShowMulti.cpp index 3a916f12c1..6fb595223f 100644 --- a/example/device/framegrabber/grabDirectShowMulti.cpp +++ b/example/device/framegrabber/grabDirectShowMulti.cpp @@ -60,7 +60,6 @@ #include // List of allowed command line options -// #define GETOPTARGS "dhn:o:" #define GETOPTARGS "c:df:hmn:io:st:?" #define GRAB_COLOR diff --git a/example/device/laserscanner/SickLDMRS-Process.cpp b/example/device/laserscanner/SickLDMRS-Process.cpp index 85ab22eb25..188424f736 100644 --- a/example/device/laserscanner/SickLDMRS-Process.cpp +++ b/example/device/laserscanner/SickLDMRS-Process.cpp @@ -142,9 +142,6 @@ void *laser_display_and_save_loop(void *) pthread_mutex_unlock(&shm_mutex); #endif - // std::cout << "laser start timestamp " - // << laserscan[0].getStartTimestamp() - time_offset << std::endl; - // Parse the four layers for (int layer = 0; layer < 4; layer++) { if (!((0x1 << layer) & layerToDisplay)) { @@ -172,8 +169,6 @@ void *laser_display_and_save_loop(void *) vpImagePoint E; // Beam echo double resolution = 5; // 100 pixels = 1 meter - increase this value to // see better near info - // std::cout << "display layer " << layer << " nb points: " - // << pointsLayer.size() << std::endl; for (unsigned int i = 0; i < pointsLayer.size(); i++) { p = pointsLayer[i]; E.set_i(height - resolution * p.getRadialDist() * cos(p.getHAngle())); diff --git a/example/device/light/ringLight.cpp b/example/device/light/ringLight.cpp index 39d96bd374..955a58ace0 100644 --- a/example/device/light/ringLight.cpp +++ b/example/device/light/ringLight.cpp @@ -75,7 +75,7 @@ during %d s.\n\ By default, that means without parameters, send a pulse which duration\n\ is fixed by the harware. To control the duration of the pulse, use \n\ \"-t \" option. To turn on the light permanently, \n\ -use \"-o -n ]\"\n \ +use \"-o -n ]\"\n\ \n\ SYNOPSIS\n\ %s [-o] [-n ] [-t ] [-h]\n\ diff --git a/example/direct-visual-servoing/photometricVisualServoingWithoutVpServo.cpp b/example/direct-visual-servoing/photometricVisualServoingWithoutVpServo.cpp index 0c178012f8..31e590054c 100644 --- a/example/direct-visual-servoing/photometricVisualServoingWithoutVpServo.cpp +++ b/example/direct-visual-servoing/photometricVisualServoingWithoutVpServo.cpp @@ -392,7 +392,7 @@ int main(int argc, const char **argv) vpColVector v; // camera velocity send to the robot // ---------------------------------------------------------- - // Minimisation + // minimization double mu; // mu = 0 : Gauss Newton ; mu != 0 : LM double lambdaGN; @@ -453,7 +453,7 @@ int main(int argc, const char **argv) { H = ((mu * diagHsd) + Hsd).inverseByLU(); } - // compute the control law + // Compute the control law e = H * Lsd.t() * error; v = -lambda * e; diff --git a/example/image/imageDiskRW.cpp b/example/image/imageDiskRW.cpp index b8325bbb74..10af918a45 100644 --- a/example/image/imageDiskRW.cpp +++ b/example/image/imageDiskRW.cpp @@ -79,7 +79,7 @@ Read and write PGM images on the disk. Also test exceptions.\n\ \n\ SYNOPSIS\n\ %s [-i ] [-o ]\n\ - [-h]\n \ + [-h]\n\ ", name); diff --git a/example/manual/ogre/HelloWorldOgre.cpp b/example/manual/ogre/HelloWorldOgre.cpp index 9ab0a164a0..96eba8cf33 100644 --- a/example/manual/ogre/HelloWorldOgre.cpp +++ b/example/manual/ogre/HelloWorldOgre.cpp @@ -109,7 +109,7 @@ int main() // Create a basic scene // ----------------------------------- - // Loading things + // Loading things // ----------------------------------- // As you will see in section 5, our // application knows locations where diff --git a/example/ogre-simulator/AROgre.cpp b/example/ogre-simulator/AROgre.cpp index 0fef65564f..2ba72b250c 100644 --- a/example/ogre-simulator/AROgre.cpp +++ b/example/ogre-simulator/AROgre.cpp @@ -510,13 +510,12 @@ void computeInitialPose(vpCameraParameters *mcam, vpImage &I, vpP } // compute the initial pose using Dementhon method followed by a non linear - // minimisation method + // minimization method // Compute initial pose mPose->computePose(vpPose::DEMENTHON_LAGRANGE_VIRTUAL_VS, *cMo); - // Display breifly just to have a glimpse a the ViSP pose - // while(cpt<500){ + // Display briefly just to have a glimpse a the ViSP pose if (opt_display) { // Display the computed pose mPose->display(I, *cMo, *mcam, 0.05, vpColor::red); diff --git a/example/ogre-simulator/AROgreBasic.cpp b/example/ogre-simulator/AROgreBasic.cpp index cd5313a0ed..17ecd07bbe 100644 --- a/example/ogre-simulator/AROgreBasic.cpp +++ b/example/ogre-simulator/AROgreBasic.cpp @@ -376,13 +376,12 @@ void computeInitialPose(vpCameraParameters *mcam, vpImage &I, vpP } // compute the initial pose using Dementhon method followed by a non linear - // minimisation method + // minimization method // Compute initial pose mPose->computePose(vpPose::DEMENTHON_LAGRANGE_VIRTUAL_VS, *cMo); - // Display breifly just to have a glimpse a the ViSP pose - // while(cpt<500){ + // Display briefly just to have a glimpse a the ViSP pose if (opt_display) { // Display the computed pose mPose->display(I, *cMo, *mcam, 0.05, vpColor::red); @@ -449,14 +448,7 @@ int main(int argc, const char **argv) return EXIT_FAILURE; } - // Declare an image, this is a gray level image (unsigned char) - // it size is not defined yet, it will be defined when the image will - // read on the disk - // vpImage I ; - - // unsigned iter = 0; std::ostringstream s; - // char cfilename[FILENAME_MAX]; if (opt_ppath.empty()) { // Set the path location of the image sequence diff --git a/example/pose-estimation/poseVirtualVS.cpp b/example/pose-estimation/poseVirtualVS.cpp index 1a4f275a9f..728e2156f9 100644 --- a/example/pose-estimation/poseVirtualVS.cpp +++ b/example/pose-estimation/poseVirtualVS.cpp @@ -510,7 +510,7 @@ int main(int argc, const char **argv) } // compute the initial pose using Dementhon method followed by a non - // linear minimisation method + // linear minimization method // Pose by Dementhon or Lagrange provides an initialization of the non linear virtual visual-servoing pose estimation pose.computePose(vpPose::DEMENTHON_LAGRANGE_VIRTUAL_VS, cMo); diff --git a/example/robot-simulator/camera/CMakeLists.txt b/example/robot-simulator/camera/CMakeLists.txt index 58fb4410f7..f5182b886f 100644 --- a/example/robot-simulator/camera/CMakeLists.txt +++ b/example/robot-simulator/camera/CMakeLists.txt @@ -104,24 +104,24 @@ foreach(cpp ${example_cpp}) endforeach() # Add test -add_test(servoSimu3D_cdMc_CamVelocity servoSimu3D_cdMc_CamVelocity ) -add_test(servoSimu3D_cdMc_CamVelocityWithoutVpServo servoSimu3D_cdMc_CamVelocityWithoutVpServo ) -add_test(servoSimu3D_cMcd_CamVelocity servoSimu3D_cMcd_CamVelocity ) -add_test(servoSimu3D_cMcd_CamVelocityWithoutVpServo servoSimu3D_cMcd_CamVelocityWithoutVpServo ) -add_test(servoSimuCircle2DCamVelocity servoSimuCircle2DCamVelocity ) -add_test(servoSimuCircle2DCamVelocityDisplay servoSimuCircle2DCamVelocityDisplay -c ${OPTION_TO_DESACTIVE_DISPLAY} ) -add_test(servoSimuCylinder2DCamVelocityDisplay servoSimuCylinder2DCamVelocityDisplay -c ${OPTION_TO_DESACTIVE_DISPLAY} ) -add_test(servoSimuFourPoints2DCamVelocity servoSimuFourPoints2DCamVelocity ) -add_test(servoSimuFourPoints2DCamVelocityDisplay servoSimuFourPoints2DCamVelocityDisplay -c ${OPTION_TO_DESACTIVE_DISPLAY} ) -add_test(servoSimuLine2DCamVelocityDisplay servoSimuLine2DCamVelocityDisplay -c ${OPTION_TO_DESACTIVE_DISPLAY} ) -add_test(servoSimuPoint2DCamVelocity1 servoSimuPoint2DCamVelocity1 ) -add_test(servoSimuPoint2DCamVelocity2 servoSimuPoint2DCamVelocity2 ) -add_test(servoSimuPoint2DCamVelocity3 servoSimuPoint2DCamVelocity3 ) -add_test(servoSimuPoint2DhalfCamVelocity1 servoSimuPoint2DhalfCamVelocity1 ) -add_test(servoSimuPoint2DhalfCamVelocity2 servoSimuPoint2DhalfCamVelocity2 ) -add_test(servoSimuPoint2DhalfCamVelocity3 servoSimuPoint2DhalfCamVelocity3 ) -add_test(servoSimuPoint3DCamVelocity servoSimuPoint3DCamVelocity ) -add_test(servoSimuSphere2DCamVelocity servoSimuSphere2DCamVelocity ) -add_test(servoSimuSphere2DCamVelocityDisplay servoSimuSphere2DCamVelocityDisplay -c ${OPTION_TO_DESACTIVE_DISPLAY} ) -add_test(servoSimuSphere2DCamVelocityDisplaySecondaryTask servoSimuSphere2DCamVelocityDisplaySecondaryTask -c ${OPTION_TO_DESACTIVE_DISPLAY} ) -add_test(servoSimuThetaUCamVelocity servoSimuThetaUCamVelocity ) +add_test(servoSimu3D_cdMc_CamVelocity servoSimu3D_cdMc_CamVelocity) +add_test(servoSimu3D_cdMc_CamVelocityWithoutVpServo servoSimu3D_cdMc_CamVelocityWithoutVpServo) +add_test(servoSimu3D_cMcd_CamVelocity servoSimu3D_cMcd_CamVelocity) +add_test(servoSimu3D_cMcd_CamVelocityWithoutVpServo servoSimu3D_cMcd_CamVelocityWithoutVpServo) +add_test(servoSimuCircle2DCamVelocity servoSimuCircle2DCamVelocity) +add_test(servoSimuCircle2DCamVelocityDisplay servoSimuCircle2DCamVelocityDisplay -c ${OPTION_TO_DESACTIVE_DISPLAY}) +add_test(servoSimuCylinder2DCamVelocityDisplay servoSimuCylinder2DCamVelocityDisplay -c ${OPTION_TO_DESACTIVE_DISPLAY}) +add_test(servoSimuFourPoints2DCamVelocity servoSimuFourPoints2DCamVelocity) +add_test(servoSimuFourPoints2DCamVelocityDisplay servoSimuFourPoints2DCamVelocityDisplay -c ${OPTION_TO_DESACTIVE_DISPLAY}) +add_test(servoSimuLine2DCamVelocityDisplay servoSimuLine2DCamVelocityDisplay -c ${OPTION_TO_DESACTIVE_DISPLAY} ) +add_test(servoSimuPoint2DCamVelocity1 servoSimuPoint2DCamVelocity1) +add_test(servoSimuPoint2DCamVelocity2 servoSimuPoint2DCamVelocity2) +add_test(servoSimuPoint2DCamVelocity3 servoSimuPoint2DCamVelocity3) +add_test(servoSimuPoint2DhalfCamVelocity1 servoSimuPoint2DhalfCamVelocity1) +add_test(servoSimuPoint2DhalfCamVelocity2 servoSimuPoint2DhalfCamVelocity2) +add_test(servoSimuPoint2DhalfCamVelocity3 servoSimuPoint2DhalfCamVelocity3) +add_test(servoSimuPoint3DCamVelocity servoSimuPoint3DCamVelocity) +add_test(servoSimuSphere2DCamVelocity servoSimuSphere2DCamVelocity) +add_test(servoSimuSphere2DCamVelocityDisplay servoSimuSphere2DCamVelocityDisplay -c ${OPTION_TO_DESACTIVE_DISPLAY}) +add_test(servoSimuSphere2DCamVelocityDisplaySecondaryTask servoSimuSphere2DCamVelocityDisplaySecondaryTask -c ${OPTION_TO_DESACTIVE_DISPLAY}) +add_test(servoSimuThetaUCamVelocity servoSimuThetaUCamVelocity) diff --git a/example/servo-afma4/moveAfma4.cpp b/example/servo-afma4/moveAfma4.cpp index 387b50aa0e..ad8444e67e 100644 --- a/example/servo-afma4/moveAfma4.cpp +++ b/example/servo-afma4/moveAfma4.cpp @@ -75,10 +75,10 @@ void usage(const char *name, const char *badparam) { fprintf(stdout, "\n\ Example of a positioning control followed by a velocity control \n\ -of the Afma4 robot.\n \ +of the Afma4 robot.\n\ \n\ SYNOPSIS\n\ - %s [-m] [-h]\n \ + %s [-m] [-h]\n\ ", name); diff --git a/example/servo-afma6/servoAfma6FourPoints2DArtVelocity.cpp b/example/servo-afma6/servoAfma6FourPoints2DArtVelocity.cpp index b40a669092..4a35a880f5 100644 --- a/example/servo-afma6/servoAfma6FourPoints2DArtVelocity.cpp +++ b/example/servo-afma6/servoAfma6FourPoints2DArtVelocity.cpp @@ -301,9 +301,6 @@ int main() // Flush the display vpDisplay::flush(I); - - // vpTRACE("\t\t || s - s* || = %f ", ( task.getError() - //).sumSquare()) ; } vpTRACE("Display task information "); diff --git a/example/servo-afma6/servoAfma6MegaposePBVS.cpp b/example/servo-afma6/servoAfma6MegaposePBVS.cpp index aa6a53ff7f..25530a7f6d 100644 --- a/example/servo-afma6/servoAfma6MegaposePBVS.cpp +++ b/example/servo-afma6/servoAfma6MegaposePBVS.cpp @@ -86,7 +86,7 @@ #include #ifdef VISP_HAVE_NLOHMANN_JSON -using json = nlohmann::json; +using json = nlohmann::json; //! json namespace shortcut #endif std::optional detectObjectForInitMegaposeClick(const vpImage &I) diff --git a/example/servo-afma6/servoAfma6Points2DCamVelocityEyeToHand.cpp b/example/servo-afma6/servoAfma6Points2DCamVelocityEyeToHand.cpp index 1ee88cadf4..1c07b79024 100644 --- a/example/servo-afma6/servoAfma6Points2DCamVelocityEyeToHand.cpp +++ b/example/servo-afma6/servoAfma6Points2DCamVelocityEyeToHand.cpp @@ -176,7 +176,7 @@ int main() } // compute the initial pose using Dementhon method followed by a non - // linear minimisation method + // linear minimization method pose.computePose(vpPose::DEMENTHON_LAGRANGE_VIRTUAL_VS, cMo); std::cout << cMo << std::endl; @@ -286,7 +286,7 @@ int main() exit(1); } - // compute the initial pose using a non linear minimisation method + // compute the initial pose using a non linear minimization method pose.clearPoint(); for (i = 0; i < nbPoint; i++) { diff --git a/example/servo-biclops/moveBiclops.cpp b/example/servo-biclops/moveBiclops.cpp index 3529ef1c1f..245afa21b1 100644 --- a/example/servo-biclops/moveBiclops.cpp +++ b/example/servo-biclops/moveBiclops.cpp @@ -78,7 +78,7 @@ void usage(const char *name, const char *badparam, std::string conf) Move the biclops robot\n\ \n\ SYNOPSIS\n\ - %s [-c ] [-h]\n \ + %s [-c ] [-h]\n\ ", name); diff --git a/example/servo-biclops/servoBiclopsPoint2DArtVelocity.cpp b/example/servo-biclops/servoBiclopsPoint2DArtVelocity.cpp index 162c3c3218..7cf6e1cce1 100644 --- a/example/servo-biclops/servoBiclopsPoint2DArtVelocity.cpp +++ b/example/servo-biclops/servoBiclopsPoint2DArtVelocity.cpp @@ -377,9 +377,6 @@ int main(int argc, const char **argv) dot.track(I); - // vpDisplay::displayCross(I,(int)dot.I(), (int)dot.J(), - // 10,vpColor::green) ; - vpFeatureBuilder::create(p, cam, dot); // get the jacobian diff --git a/example/servo-ptu46/servoPtu46Point2DArtVelocity.cpp b/example/servo-ptu46/servoPtu46Point2DArtVelocity.cpp index d71a1a0fec..def4c02299 100644 --- a/example/servo-ptu46/servoPtu46Point2DArtVelocity.cpp +++ b/example/servo-ptu46/servoPtu46Point2DArtVelocity.cpp @@ -228,9 +228,6 @@ int main() dot.track(I); - // vpDisplay::displayCross(I,(int)dot.I(), (int)dot.J(), - // 10,vpColor::green) ; - vpFeatureBuilder::create(p, cam, dot); // get the jacobian diff --git a/example/servo-viper650/servoViper650FourPoints2DArtVelocityLs_cur.cpp b/example/servo-viper650/servoViper650FourPoints2DArtVelocityLs_cur.cpp index ff3a8a7968..414b834aff 100644 --- a/example/servo-viper650/servoViper650FourPoints2DArtVelocityLs_cur.cpp +++ b/example/servo-viper650/servoViper650FourPoints2DArtVelocityLs_cur.cpp @@ -280,7 +280,7 @@ int main() } // During the servo, we compute the pose using LOWE method. For the - // initial pose used in the non linear minimisation we use the pose + // initial pose used in the non linear minimization we use the pose // computed at the previous iteration. compute_pose(point, dot, cam, cMo, false); diff --git a/example/servo-viper650/servoViper650FourPoints2DCamVelocityLs_cur-SR300.cpp b/example/servo-viper650/servoViper650FourPoints2DCamVelocityLs_cur-SR300.cpp index ed87efaaa3..fc24418c70 100644 --- a/example/servo-viper650/servoViper650FourPoints2DCamVelocityLs_cur-SR300.cpp +++ b/example/servo-viper650/servoViper650FourPoints2DCamVelocityLs_cur-SR300.cpp @@ -275,7 +275,7 @@ int main() } // During the servo, we compute the pose using a non linear method. For - // the initial pose used in the non linear minimisation we use the pose + // the initial pose used in the non linear minimization we use the pose // computed at the previous iteration. compute_pose(point, dot, cam, cMo, false); diff --git a/example/servo-viper650/servoViper650FourPoints2DCamVelocityLs_cur.cpp b/example/servo-viper650/servoViper650FourPoints2DCamVelocityLs_cur.cpp index e011f94706..b0eee66934 100644 --- a/example/servo-viper650/servoViper650FourPoints2DCamVelocityLs_cur.cpp +++ b/example/servo-viper650/servoViper650FourPoints2DCamVelocityLs_cur.cpp @@ -270,7 +270,7 @@ int main() } // During the servo, we compute the pose using a non linear method. For - // the initial pose used in the non linear minimisation we use the pose + // the initial pose used in the non linear minimization we use the pose // computed at the previous iteration. compute_pose(point, dot, cam, cMo, false); diff --git a/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-basic.cpp b/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-basic.cpp index 8e99005b13..b09f9e0a49 100644 --- a/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-basic.cpp +++ b/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-basic.cpp @@ -337,7 +337,6 @@ int main() unsigned int k = 0; for (unsigned int j = 0; j < 6; j++) // j is the joint - // if (pb[j]==1) { if (std::fabs(pb[j] - 1) <= std::numeric_limits::epsilon()) { for (unsigned int i = 0; i < dimKernelL; i++) E[k][i] = kernelJ1[j][i]; diff --git a/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-gpa.cpp b/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-gpa.cpp index 52725a9f78..e966f7e772 100644 --- a/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-gpa.cpp +++ b/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-gpa.cpp @@ -284,7 +284,6 @@ int main() } h_s = beta * h_s / 2.0; // cost function e2 *= beta; - // std::cout << e2.t() << std::endl; std::cout << "Cost function h_s: " << h_s << std::endl; sec_task = task.secondaryTask(e2, de2dt); diff --git a/example/video/videoReader.cpp b/example/video/videoReader.cpp index 9e94015a1a..9b64d22d65 100644 --- a/example/video/videoReader.cpp +++ b/example/video/videoReader.cpp @@ -81,7 +81,7 @@ Read a video file on the disk.\n\ \n\ SYNOPSIS\n\ %s [-i ] \n\ - [-h]\n \ + [-h]\n\ ", name); diff --git a/modules/ar/include/visp3/ar/vpSimulatorException.h b/modules/ar/include/visp3/ar/vpSimulatorException.h index d09a88ad56..e5a75bffe8 100644 --- a/modules/ar/include/visp3/ar/vpSimulatorException.h +++ b/modules/ar/include/visp3/ar/vpSimulatorException.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,16 +29,15 @@ * * Description: * Exceptions that can be emitted by the simulator classes. - * -*****************************************************************************/ + */ #ifndef _vpSimulatorException_h_ #define _vpSimulatorException_h_ -/* \file vpSimulatorException.h - \brief Error that can be emitted by the vpSimulator class and its derivatives +/*! + * \file vpSimulatorException.h + * \brief Error that can be emitted by the vpSimulator class and its derivatives */ -/* Classes standards. */ #include @@ -48,18 +46,18 @@ #include /* Classe string. */ /*! - - \class vpSimulatorException - \brief Error that can be emitted by the vpSimulator class and its derivatives. + * \class vpSimulatorException + * \brief Error that can be emitted by the vpSimulator class and its derivatives. */ class VISP_EXPORT vpSimulatorException : public vpException { public: /*! - \brief Lists the possible error than can be emitted while calling - vpSimulator member - */ - enum errorSimulatorCodeEnum { + * Lists the possible error than can be emitted while calling + * vpSimulator member + */ + enum errorSimulatorCodeEnum + { ioError, //!< I/O error noFileNameError, //!< Filename error notInitializedError, //!< Initialization error @@ -68,8 +66,19 @@ class VISP_EXPORT vpSimulatorException : public vpException }; public: + /*! + * Constructor. + */ vpSimulatorException(int id, const char *format, ...); + + /*! + * Constructor. + */ vpSimulatorException(int id, const std::string &msg); + + /*! + * Constructor. + */ explicit vpSimulatorException(int id); }; diff --git a/modules/ar/src/ogre-simulator/vpAROgre.cpp b/modules/ar/src/ogre-simulator/vpAROgre.cpp index 235cd6ef48..bbd4cf952c 100644 --- a/modules/ar/src/ogre-simulator/vpAROgre.cpp +++ b/modules/ar/src/ogre-simulator/vpAROgre.cpp @@ -382,11 +382,11 @@ void vpAROgre::init(bool //----------------------------------------------------- // 4 Create the SceneManager // - // ST_GENERIC = octree - // ST_EXTERIOR_CLOSE = simple terrain - // ST_EXTERIOR_FAR = nature terrain (depreciated) - // ST_EXTERIOR_REAL_FAR = paging landscape - // ST_INTERIOR = Quake3 BSP + // ST_GENERIC = octree + // ST_EXTERIOR_CLOSE = simple terrain + // ST_EXTERIOR_FAR = nature terrain (depreciated) + // ST_EXTERIOR_REAL_FAR = paging landscape + // ST_INTERIOR = Quake3 BSP //----------------------------------------------------- mSceneMgr = mRoot->createSceneManager(Ogre::ST_GENERIC); diff --git a/modules/core/include/visp3/core/vpArray2D.h b/modules/core/include/visp3/core/vpArray2D.h index ccf05299d2..b3248c656f 100644 --- a/modules/core/include/visp3/core/vpArray2D.h +++ b/modules/core/include/visp3/core/vpArray2D.h @@ -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: * This class implements an 2D array as a template class. - * -*****************************************************************************/ + */ #ifndef _vpArray2D_h_ #define _vpArray2D_h_ diff --git a/modules/core/include/visp3/core/vpBSpline.h b/modules/core/include/visp3/core/vpBSpline.h index ee2ebefa53..29debfaf87 100644 --- a/modules/core/include/visp3/core/vpBSpline.h +++ b/modules/core/include/visp3/core/vpBSpline.h @@ -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: * This class implements the B-Spline - * -*****************************************************************************/ + */ #ifndef vpBSpline_H #define vpBSpline_H diff --git a/modules/core/include/visp3/core/vpCPUFeatures.h b/modules/core/include/visp3/core/vpCPUFeatures.h index 14a10a8653..1fe8d37c2a 100644 --- a/modules/core/include/visp3/core/vpCPUFeatures.h +++ b/modules/core/include/visp3/core/vpCPUFeatures.h @@ -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: * CPU features (hardware capabilities). - * -*****************************************************************************/ + */ #ifndef _vpCPUFeatures_h_ #define _vpCPUFeatures_h_ diff --git a/modules/core/include/visp3/core/vpCameraParameters.h b/modules/core/include/visp3/core/vpCameraParameters.h index 5bc255eccf..06b36d5e25 100644 --- a/modules/core/include/visp3/core/vpCameraParameters.h +++ b/modules/core/include/visp3/core/vpCameraParameters.h @@ -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: * Camera intrinsic parameters. - * -*****************************************************************************/ + */ /*! \file vpCameraParameters.h @@ -292,8 +290,8 @@ \code{.unparsed} Read camera parameters from cam.json: Camera parameters for perspective projection without distortion: - px = 801 py = 802 - u0 = 325 v0 = 245 + px = 801 py = 802 + u0 = 325 v0 = 245 \endcode The content of the `cam.json` file is the following: diff --git a/modules/core/include/visp3/core/vpCannyEdgeDetection.h b/modules/core/include/visp3/core/vpCannyEdgeDetection.h index e75e95807e..0ca547ddac 100644 --- a/modules/core/include/visp3/core/vpCannyEdgeDetection.h +++ b/modules/core/include/visp3/core/vpCannyEdgeDetection.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -27,8 +26,7 @@ * * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - * -*****************************************************************************/ + */ #ifndef _vpCannyEdgeDetection_h_ #define _vpCannyEdgeDetection_h_ @@ -45,7 +43,7 @@ // 3rd parties include #ifdef VISP_HAVE_NLOHMANN_JSON #include -using json = nlohmann::json; +using json = nlohmann::json; //! json namespace shortcut #endif class VISP_EXPORT vpCannyEdgeDetection diff --git a/modules/core/include/visp3/core/vpCircle.h b/modules/core/include/visp3/core/vpCircle.h index 567efc23be..3407516fdc 100644 --- a/modules/core/include/visp3/core/vpCircle.h +++ b/modules/core/include/visp3/core/vpCircle.h @@ -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: * Visual feature circle. - * -*****************************************************************************/ + */ /*! \file vpCircle.h diff --git a/modules/core/include/visp3/core/vpClient.h b/modules/core/include/visp3/core/vpClient.h index 0c24b5e0a3..10ae2923ad 100644 --- a/modules/core/include/visp3/core/vpClient.h +++ b/modules/core/include/visp3/core/vpClient.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * TCP Client - * - * Authors: - * Aurelien Yol - * -*****************************************************************************/ + */ #ifndef vpClient_H #define vpClient_H diff --git a/modules/core/include/visp3/core/vpColVector.h b/modules/core/include/visp3/core/vpColVector.h index 5133b4c640..1ba64d3fe6 100644 --- a/modules/core/include/visp3/core/vpColVector.h +++ b/modules/core/include/visp3/core/vpColVector.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,10 +29,7 @@ * * Description: * Provide some simple operation on column vectors. - * - * Authors: - * -*****************************************************************************/ + */ #ifndef _vpColVector_h_ #define _vpColVector_h_ diff --git a/modules/core/include/visp3/core/vpColor.h b/modules/core/include/visp3/core/vpColor.h index 114f5f7a65..509a736aa8 100644 --- a/modules/core/include/visp3/core/vpColor.h +++ b/modules/core/include/visp3/core/vpColor.h @@ -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: * Color definition. - * -*****************************************************************************/ + */ #ifndef vpColor_hh #define vpColor_hh diff --git a/modules/core/include/visp3/core/vpColorDepthConversion.h b/modules/core/include/visp3/core/vpColorDepthConversion.h index 7570e1f43b..5b34e0739d 100644 --- a/modules/core/include/visp3/core/vpColorDepthConversion.h +++ b/modules/core/include/visp3/core/vpColorDepthConversion.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Color to Depth conversion. - * - * Authors: - * Julien Dufour - * -*****************************************************************************/ + */ #pragma once diff --git a/modules/core/include/visp3/core/vpColormap.h b/modules/core/include/visp3/core/vpColormap.h index f04babc34a..e30ddd189c 100644 --- a/modules/core/include/visp3/core/vpColormap.h +++ b/modules/core/include/visp3/core/vpColormap.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -31,8 +30,7 @@ * Description: * Colormap class to recolor an image with different grayscale values into * some corresponding color values, for better visualization for example. - * -*****************************************************************************/ + */ #ifndef _vpColormap_h_ #define _vpColormap_h_ diff --git a/modules/core/include/visp3/core/vpConvert.h b/modules/core/include/visp3/core/vpConvert.h index e4de413125..b87095f7bc 100644 --- a/modules/core/include/visp3/core/vpConvert.h +++ b/modules/core/include/visp3/core/vpConvert.h @@ -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: * Directory management. - * -*****************************************************************************/ + */ #ifndef _vpConvert_h_ #define _vpConvert_h_ diff --git a/modules/core/include/visp3/core/vpCylinder.h b/modules/core/include/visp3/core/vpCylinder.h index 1feebd16e8..5347c78658 100644 --- a/modules/core/include/visp3/core/vpCylinder.h +++ b/modules/core/include/visp3/core/vpCylinder.h @@ -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: * Cylinder feature. - * -*****************************************************************************/ + */ /*! \file vpCylinder.h diff --git a/modules/core/include/visp3/core/vpDebug.h b/modules/core/include/visp3/core/vpDebug.h index 9712030e82..abce2f9162 100644 --- a/modules/core/include/visp3/core/vpDebug.h +++ b/modules/core/include/visp3/core/vpDebug.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -32,21 +31,17 @@ * Debug and trace macro. * * - TRACING: vpTRACE and vpERROR_TRACE work like printf with carreer - *return at the end of the string. vpCERROR et vpCTRACE work like the C++ - *output streams std::cout and std::cerr. + * return at the end of the string. vpCERROR et vpCTRACE work like the C++ + * output streams std::cout and std::cerr. * - DEBUGING: vpDEBUG_TRACE(niv) and vpDERROR_TRACE(niv), work like - *printf, but print only if the tracing level niv is greater than the debug - *level VP_DEBUG_MODE. vpCDEBUG(niv) work like the C++ output - *stream std::cout. vpDEBUG_ENABLE(niv) is equal to 1 if the - *debug level niv is greater than the debug mode + * printf, but print only if the tracing level niv is greater than the debug + * level VP_DEBUG_MODE. vpCDEBUG(niv) work like the C++ output + * stream std::cout. vpDEBUG_ENABLE(niv) is equal to 1 if the + * debug level niv is greater than the debug mode * VP_DEBUG_MODE, 0 else. * - PROG DEFENSIVE: DEFENSIF(a) is equal to a if defensive mode is active, - *0 else. - * - * Authors: - * Nicolas Mansard, Bruno Renier - * -*****************************************************************************/ + * 0 else. + */ #ifndef _vpDebug_h_ #define _vpDebug_h_ diff --git a/modules/core/include/visp3/core/vpDisplay.h b/modules/core/include/visp3/core/vpDisplay.h index f45d4f8539..326a7e8ea4 100644 --- a/modules/core/include/visp3/core/vpDisplay.h +++ b/modules/core/include/visp3/core/vpDisplay.h @@ -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: * Image display. - * -*****************************************************************************/ + */ #ifndef _vpDisplay_h_ #define _vpDisplay_h_ diff --git a/modules/core/include/visp3/core/vpDisplayException.h b/modules/core/include/visp3/core/vpDisplayException.h index 1156e50248..4214b7eaf3 100644 --- a/modules/core/include/visp3/core/vpDisplayException.h +++ b/modules/core/include/visp3/core/vpDisplayException.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,58 +29,48 @@ * * Description: * Exception that can be emitted by the vpDisplay class and its derivatives. - * -*****************************************************************************/ + */ #ifndef _vpDisplayException_h_ #define _vpDisplayException_h_ -/* ------------------------------------------------------------------------- - */ -/* --- INCLUDE ------------------------------------------------------------- - */ -/* ------------------------------------------------------------------------- +/*! + * \file vpDisplayException.h + * \brief error that can be emitted by the vpDisplay class and its derivatives */ -/* \file vpDisplayException.h - \brief error that can be emitted by the vpDisplay class and its derivatives - */ -/* Classes standards. */ -#include /* Classe std::ostream. */ -#include /* Classe string. */ +#include +#include #include #include -/* ------------------------------------------------------------------------- - */ -/* --- CLASS --------------------------------------------------------------- - */ -/* ------------------------------------------------------------------------- - */ - /*! - \class vpDisplayException - \ingroup group_core_debug - \brief Error that can be emitted by the vpDisplay class and its derivatives. + * \class vpDisplayException + * \ingroup group_core_debug + * \brief Error that can be emitted by the vpDisplay class and its derivatives. */ class VISP_EXPORT vpDisplayException : public vpException { public: /*! - \brief Lists the possible error than can be emitted while calling - vpDisplay member - */ - enum errorDisplayCodeEnum { - notInitializedError, - cannotOpenWindowError, - connexionError, - XWindowsError, - GTKWindowsError, - colorAllocError, - depthNotSupportedError + * Lists the possible error than can be emitted while calling + * vpDisplay member + */ + enum errorDisplayCodeEnum + { + notInitializedError, //!< Display not initialized + cannotOpenWindowError, //!< Unable to open display window + connexionError, //!< Connection error + XWindowsError, //!< XWindow error + GTKWindowsError, //!< GTK error + colorAllocError, //!< Color allocation error + depthNotSupportedError //!< Color depth not supported }; public: + /*! + * Constructor. + */ vpDisplayException(int id, const char *format, ...) { this->code = id; @@ -91,9 +80,15 @@ class VISP_EXPORT vpDisplayException : public vpException va_end(args); } - vpDisplayException(int id, const std::string &msg) : vpException(id, msg) {} + /*! + * Constructor. + */ + vpDisplayException(int id, const std::string &msg) : vpException(id, msg) { } - explicit vpDisplayException(int id) : vpException(id) {} + /*! + * Constructor. + */ + explicit vpDisplayException(int id) : vpException(id) { } }; #endif diff --git a/modules/core/include/visp3/core/vpEigenConversion.h b/modules/core/include/visp3/core/vpEigenConversion.h index 01d206a341..f1264eaccb 100644 --- a/modules/core/include/visp3/core/vpEigenConversion.h +++ b/modules/core/include/visp3/core/vpEigenConversion.h @@ -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: * ViSP <--> Eigen conversion. - * -*****************************************************************************/ + */ #ifndef _vpEigenConversion_h_ #define _vpEigenConversion_h_ diff --git a/modules/core/include/visp3/core/vpEndian.h b/modules/core/include/visp3/core/vpEndian.h index 6f4a56bd0c..a55ec06854 100644 --- a/modules/core/include/visp3/core/vpEndian.h +++ b/modules/core/include/visp3/core/vpEndian.h @@ -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: * Determine machine endianness and define VISP_LITTLE_ENDIAN, VISP_BIG_ENDIAN and VISP_PDP_ENDIAN macros. - * -*****************************************************************************/ + */ /*! \file vpEndian.h diff --git a/modules/core/include/visp3/core/vpException.h b/modules/core/include/visp3/core/vpException.h index 7094d2c8e6..70b74a51bd 100644 --- a/modules/core/include/visp3/core/vpException.h +++ b/modules/core/include/visp3/core/vpException.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,43 +29,31 @@ * * Description: * Exception handling. - * - * Authors: - * Nicolas Mansard - * -*****************************************************************************/ + */ -/* \file vpException.h - \brief error that can be emitted by the vp class and its derivatives +/*! + * \file vpException.h + * \brief error that can be emitted by the vp class and its derivatives */ #ifndef _vpException_h_ #define _vpException_h_ -/* --------------------------------------------------------------------- */ -/* --- INCLUDE --------------------------------------------------------- */ -/* --------------------------------------------------------------------- */ - #include -/* Classes standards. */ -#include /* Classe std::ostream. */ +#include #include -#include /* Classe string. */ - -/* --------------------------------------------------------------------- */ -/* --- CLASS ----------------------------------------------------------- */ -/* --------------------------------------------------------------------- */ +#include /*! - \class vpException - \ingroup group_core_debug - \brief error that can be emitted by ViSP classes. - - This class inherites from the standard std::exception contained in the C++ - STL. - It is therefore possible to catch vpException with any other derivative of - std::exception in the same catch. + * \class vpException + * \ingroup group_core_debug + * \brief error that can be emitted by ViSP classes. + * + * This class inherits from the standard std::exception contained in the C++ + * STL. + * It is therefore possible to catch vpException with any other derivative of + * std::exception in the same catch. */ class VISP_EXPORT vpException : public std::exception { @@ -81,10 +68,11 @@ class VISP_EXPORT vpException : public std::exception void setMessage(const char *format, va_list args); //! forbid the empty constructor (protected) - vpException() : code(notInitialized), message(""){}; + vpException() : code(notInitialized), message("") { }; public: - enum generalExceptionEnum { + enum generalExceptionEnum + { memoryAllocationError, //!< Memory allocation error memoryFreeError, //!< Memory free error functionNotImplementedError, //!< Function not implemented @@ -98,37 +86,66 @@ class VISP_EXPORT vpException : public std::exception notInitialized //!< Used to indicate that a parameter is not initialized. }; + /*! + * Constructor. + */ vpException(int code, const char *format, va_list args); + /*! + * Constructor. + */ vpException(int code, const char *format, ...); + + /*! + * Constructor. + */ vpException(int code, const std::string &msg); + + /*! + * Constructor. + */ explicit vpException(int code); /*! - Basic destructor. Do nothing but implemented to fit the inheritance from - std::exception - */ + * Destructor. Do nothing but implemented to fit the inheritance from + * std::exception + */ #if VISP_CXX_STANDARD > VISP_CXX_STANDARD_98 - virtual ~vpException() {} + virtual ~vpException() { } #else - virtual ~vpException() throw() {} + virtual ~vpException() throw() { } #endif /** @name Inherited functionalities from vpException */ //@{ - //! Send the object code. + /*! + * Send the object code. + */ int getCode() const; - //! Send a reference (constant) related the error message (can be empty). + /*! + * Send a reference (constant) related the error message (can be empty). + */ const std::string &getStringMessage() const; - //! send a pointer on the array of \e char related to the error string. - //! Cannot be \e NULL. + + /*! + * Send a pointer on the array of \e char related to the error string. + * Cannot be \e NULL. + */ const char *getMessage() const; + + /*! + * Overloading of the what() method of std::exception to return the vpException + * message. + * + * \return pointer on the array of \e char related to the error string. + */ + const char *what() const throw(); //@} - //! Print the error structure. + /*! + * Print the error structure. + */ friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpException &art); - - const char *what() const throw(); }; -#endif /* #ifndef _vpException_h_ */ +#endif diff --git a/modules/core/include/visp3/core/vpExponentialMap.h b/modules/core/include/visp3/core/vpExponentialMap.h index a1a7c03697..eae27af3b0 100644 --- a/modules/core/include/visp3/core/vpExponentialMap.h +++ b/modules/core/include/visp3/core/vpExponentialMap.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Exponential map. - * - * Authors: - * Francois Chaumette - * -*****************************************************************************/ + */ /*! \file vpExponentialMap.h diff --git a/modules/core/include/visp3/core/vpFeatureDisplay.h b/modules/core/include/visp3/core/vpFeatureDisplay.h index 6f53be3c17..66c3af9643 100644 --- a/modules/core/include/visp3/core/vpFeatureDisplay.h +++ b/modules/core/include/visp3/core/vpFeatureDisplay.h @@ -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: * Interface with the image for feature display. - * -*****************************************************************************/ + */ #ifndef vpFeatureDisplay_H #define vpFeatureDisplay_H diff --git a/modules/core/include/visp3/core/vpFont.h b/modules/core/include/visp3/core/vpFont.h index b3194b1ae6..39031505c2 100644 --- a/modules/core/include/visp3/core/vpFont.h +++ b/modules/core/include/visp3/core/vpFont.h @@ -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: * Draw text in an image. - * -*****************************************************************************/ + */ #ifndef _vpFont_h_ #define _vpFont_h_ diff --git a/modules/core/include/visp3/core/vpForceTwistMatrix.h b/modules/core/include/visp3/core/vpForceTwistMatrix.h index 0b216e72f7..993e66ef33 100644 --- a/modules/core/include/visp3/core/vpForceTwistMatrix.h +++ b/modules/core/include/visp3/core/vpForceTwistMatrix.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -31,8 +30,7 @@ * Description: * Twist transformation matrix that allows to transform forces from one * frame to an other. - * -*****************************************************************************/ + */ #ifndef vpForceTwistMatrix_h #define vpForceTwistMatrix_h diff --git a/modules/core/include/visp3/core/vpForwardProjection.h b/modules/core/include/visp3/core/vpForwardProjection.h index e6d9534855..3cf257c87b 100644 --- a/modules/core/include/visp3/core/vpForwardProjection.h +++ b/modules/core/include/visp3/core/vpForwardProjection.h @@ -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: * Forward projection. - * -*****************************************************************************/ + */ #ifndef vpForwardProjection_H #define vpForwardProjection_H diff --git a/modules/core/include/visp3/core/vpFrameGrabber.h b/modules/core/include/visp3/core/vpFrameGrabber.h index 43e50b93d2..0806476bd0 100644 --- a/modules/core/include/visp3/core/vpFrameGrabber.h +++ b/modules/core/include/visp3/core/vpFrameGrabber.h @@ -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: * Frame grabbing. - * -*****************************************************************************/ + */ #ifndef vpFrameGrabber_hh #define vpFrameGrabber_hh diff --git a/modules/core/include/visp3/core/vpFrameGrabberException.h b/modules/core/include/visp3/core/vpFrameGrabberException.h index 3254507e93..1229b44cb5 100644 --- a/modules/core/include/visp3/core/vpFrameGrabberException.h +++ b/modules/core/include/visp3/core/vpFrameGrabberException.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,52 +29,46 @@ * * Description: * Exceptions that can be emitted by the vpFrameGrabber class and its - *derivates. - * -*****************************************************************************/ + * derivates. + */ #ifndef _vpFrameGrabberException_h_ #define _vpFrameGrabberException_h_ -/* ------------------------------------------------------------------------- - */ -/* --- INCLUDE ------------------------------------------------------------- - */ -/* ------------------------------------------------------------------------- - */ - -/* \file vpFrameGrabberException.h - \brief error that can be emitted by the vpFrameGrabber class and its - derivates +/*! + * \file vpFrameGrabberException.h + * \brief error that can be emitted by the vpFrameGrabber class and its + * derivates */ -/* Classes standards. */ -#include /* Classe std::ostream. */ -#include /* Classe string. */ +#include +#include #include #include -/* ------------------------------------------------------------------------- - */ -/* --- CLASS --------------------------------------------------------------- - */ -/* ------------------------------------------------------------------------- - */ /*! - \brief Error that can be emitted by the vpFrameGrabber class and its - derivates + * \brief Error that can be emitted by the vpFrameGrabber class and its + * derivates. */ class VISP_EXPORT vpFrameGrabberException : public vpException { public: /*! - \brief Lists the possible error than can be emitted while calling - vpFrameGrabber member - */ - enum errorFrameGrabberCodeEnum { settingError, initializationError, otherError }; + * Lists the possible error than can be emitted while calling + * vpFrameGrabber member + */ + enum errorFrameGrabberCodeEnum + { + settingError, //!< Grabber settings error + initializationError, //!< Grabber initialization error + otherError //!< Grabber returned an other error + }; public: + /*! + * Constructor. + */ vpFrameGrabberException(int id, const char *format, ...) { this->code = id; @@ -84,8 +77,16 @@ class VISP_EXPORT vpFrameGrabberException : public vpException setMessage(format, args); va_end(args); } + + /*! + * Constructor. + */ vpFrameGrabberException(int id, const std::string &msg) : vpException(id, msg) { } + + /*! + * Constructor. + */ explicit vpFrameGrabberException(int id) : vpException(id) { } }; -#endif /* #ifndef _vpFrameGrabberException_h_ */ +#endif diff --git a/modules/core/include/visp3/core/vpGEMM.h b/modules/core/include/visp3/core/vpGEMM.h index b7f88bb9a3..4e6a9e4ba6 100644 --- a/modules/core/include/visp3/core/vpGEMM.h +++ b/modules/core/include/visp3/core/vpGEMM.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Matrix generalized multiplication. - * - * Authors: - * Laneurit Jean - * -*****************************************************************************/ + */ #ifndef _vpGEMM_h_ #define _vpGEMM_h_ diff --git a/modules/core/include/visp3/core/vpGaussRand.h b/modules/core/include/visp3/core/vpGaussRand.h index 6a0dd49e68..79c5f1d356 100644 --- a/modules/core/include/visp3/core/vpGaussRand.h +++ b/modules/core/include/visp3/core/vpGaussRand.h @@ -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: * Generation of random number with uniform and normal probability density. - * -*****************************************************************************/ + */ #ifndef vpGaussRand_hh #define vpGaussRand_hh diff --git a/modules/core/include/visp3/core/vpGaussianFilter.h b/modules/core/include/visp3/core/vpGaussianFilter.h index d559e35026..5795ea4530 100644 --- a/modules/core/include/visp3/core/vpGaussianFilter.h +++ b/modules/core/include/visp3/core/vpGaussianFilter.h @@ -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: * Gaussian filter class. - * -*****************************************************************************/ + */ /*! \file vpGaussianFilter.h diff --git a/modules/core/include/visp3/core/vpHinkley.h b/modules/core/include/visp3/core/vpHinkley.h index 6445571090..d24760c2d6 100644 --- a/modules/core/include/visp3/core/vpHinkley.h +++ b/modules/core/include/visp3/core/vpHinkley.h @@ -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: * Hinkley's cumulative sum test implementation. - * -*****************************************************************************/ + */ #ifndef vpHinkley_H #define vpHinkley_H diff --git a/modules/core/include/visp3/core/vpHistogram.h b/modules/core/include/visp3/core/vpHistogram.h index 9741606ed1..36afaeaec2 100644 --- a/modules/core/include/visp3/core/vpHistogram.h +++ b/modules/core/include/visp3/core/vpHistogram.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,9 +29,7 @@ * * Description: * Gray level histogram manipulation. - * - * -*****************************************************************************/ + */ /*! \file vpHistogram.h diff --git a/modules/core/include/visp3/core/vpHistogramPeak.h b/modules/core/include/visp3/core/vpHistogramPeak.h index 02cbf87a36..8d5f63eb89 100644 --- a/modules/core/include/visp3/core/vpHistogramPeak.h +++ b/modules/core/include/visp3/core/vpHistogramPeak.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,15 +29,12 @@ * * Description: * Gray level histogram manipulation. - * - * -*****************************************************************************/ + */ /*! \file vpHistogramPeak.h \brief Declaration of the vpHistogramPeak class. Class vpHistogramPeak defines a gray level histogram peak. - */ #ifndef vpHistogramPeak_h diff --git a/modules/core/include/visp3/core/vpHistogramValey.h b/modules/core/include/visp3/core/vpHistogramValey.h index 1fdb828da2..d4ab3f463c 100644 --- a/modules/core/include/visp3/core/vpHistogramValey.h +++ b/modules/core/include/visp3/core/vpHistogramValey.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,9 +29,7 @@ * * Description: * Gray level histogram manipulation. - * - * -*****************************************************************************/ + */ /*! \file vpHistogramValey.h diff --git a/modules/core/include/visp3/core/vpHomogeneousMatrix.h b/modules/core/include/visp3/core/vpHomogeneousMatrix.h index 2d290a04f8..2468493586 100644 --- a/modules/core/include/visp3/core/vpHomogeneousMatrix.h +++ b/modules/core/include/visp3/core/vpHomogeneousMatrix.h @@ -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: * Homogeneous matrix. - * -*****************************************************************************/ + */ /*! \file vpHomogeneousMatrix.h diff --git a/modules/core/include/visp3/core/vpImage.h b/modules/core/include/visp3/core/vpImage.h index 1f9eebcffc..0f56a92b36 100644 --- a/modules/core/include/visp3/core/vpImage.h +++ b/modules/core/include/visp3/core/vpImage.h @@ -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: * Image handling. - * -*****************************************************************************/ + */ /*! \file vpImage.h @@ -486,7 +484,7 @@ inline std::ostream &operator<<(std::ostream &s, const vpImage &I) #if defined(VISP_HAVE_PTHREAD) || (defined(_WIN32) && !defined(WINRT_8_0)) namespace { -struct ImageLut_Param_t +struct vpImageLut_Param_t { unsigned int m_start_index; unsigned int m_end_index; @@ -494,16 +492,16 @@ struct ImageLut_Param_t unsigned char m_lut[256]; unsigned char *m_bitmap; - ImageLut_Param_t() : m_start_index(0), m_end_index(0), m_lut(), m_bitmap(NULL) { } + vpImageLut_Param_t() : m_start_index(0), m_end_index(0), m_lut(), m_bitmap(NULL) { } - ImageLut_Param_t(unsigned int start_index, unsigned int end_index, unsigned char *bitmap) + vpImageLut_Param_t(unsigned int start_index, unsigned int end_index, unsigned char *bitmap) : m_start_index(start_index), m_end_index(end_index), m_lut(), m_bitmap(bitmap) { } }; vpThread::Return performLutThread(vpThread::Args args) { - ImageLut_Param_t *imageLut_param = static_cast(args); + vpImageLut_Param_t *imageLut_param = static_cast(args); unsigned int start_index = imageLut_param->m_start_index; unsigned int end_index = imageLut_param->m_end_index; @@ -554,7 +552,7 @@ vpThread::Return performLutThread(vpThread::Args args) return 0; } -struct ImageLutRGBa_Param_t +struct vpImageLutRGBa_Param_t { unsigned int m_start_index; unsigned int m_end_index; @@ -562,16 +560,16 @@ struct ImageLutRGBa_Param_t vpRGBa m_lut[256]; unsigned char *m_bitmap; - ImageLutRGBa_Param_t() : m_start_index(0), m_end_index(0), m_lut(), m_bitmap(NULL) { } + vpImageLutRGBa_Param_t() : m_start_index(0), m_end_index(0), m_lut(), m_bitmap(NULL) { } - ImageLutRGBa_Param_t(unsigned int start_index, unsigned int end_index, unsigned char *bitmap) + vpImageLutRGBa_Param_t(unsigned int start_index, unsigned int end_index, unsigned char *bitmap) : m_start_index(start_index), m_end_index(end_index), m_lut(), m_bitmap(bitmap) { } }; vpThread::Return performLutRGBaThread(vpThread::Args args) { - ImageLutRGBa_Param_t *imageLut_param = static_cast(args); + vpImageLutRGBa_Param_t *imageLut_param = static_cast(args); unsigned int start_index = imageLut_param->m_start_index; unsigned int end_index = imageLut_param->m_end_index; @@ -2060,7 +2058,7 @@ template <> inline void vpImage::performLut(const unsigned char(& // Multi-threads std::vector threadpool; - std::vector imageLutParams; + std::vector imageLutParams; unsigned int image_size = getSize(); unsigned int step = image_size / nbThreads; @@ -2074,7 +2072,7 @@ template <> inline void vpImage::performLut(const unsigned char(& end_index = start_index + last_step; } - ImageLut_Param_t *imageLut_param = new ImageLut_Param_t(start_index, end_index, bitmap); + vpImageLut_Param_t *imageLut_param = new vpImageLut_Param_t(start_index, end_index, bitmap); memcpy(imageLut_param->m_lut, lut, 256 * sizeof(unsigned char)); imageLutParams.push_back(imageLut_param); @@ -2147,7 +2145,7 @@ template <> inline void vpImage::performLut(const vpRGBa(&lut)[256], uns #if defined(VISP_HAVE_PTHREAD) || (defined(_WIN32) && !defined(WINRT_8_0)) // Multi-threads std::vector threadpool; - std::vector imageLutParams; + std::vector imageLutParams; unsigned int image_size = getSize(); unsigned int step = image_size / nbThreads; @@ -2161,7 +2159,7 @@ template <> inline void vpImage::performLut(const vpRGBa(&lut)[256], uns end_index = start_index + last_step; } - ImageLutRGBa_Param_t *imageLut_param = new ImageLutRGBa_Param_t(start_index, end_index, (unsigned char *)bitmap); + vpImageLutRGBa_Param_t *imageLut_param = new vpImageLutRGBa_Param_t(start_index, end_index, (unsigned char *)bitmap); memcpy(static_cast(imageLut_param->m_lut), lut, 256 * sizeof(vpRGBa)); imageLutParams.push_back(imageLut_param); diff --git a/modules/core/include/visp3/core/vpImageCircle.h b/modules/core/include/visp3/core/vpImageCircle.h index e3648b7259..6f1150f0c2 100644 --- a/modules/core/include/visp3/core/vpImageCircle.h +++ b/modules/core/include/visp3/core/vpImageCircle.h @@ -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: * Image circle, i.e. circle in the image space. - * -*****************************************************************************/ + */ /*! \file vpImageCircle.h diff --git a/modules/core/include/visp3/core/vpImageConvert.h b/modules/core/include/visp3/core/vpImageConvert.h index a9c0039d36..2c7c9c57fb 100644 --- a/modules/core/include/visp3/core/vpImageConvert.h +++ b/modules/core/include/visp3/core/vpImageConvert.h @@ -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: * Convert image types. - * -*****************************************************************************/ + */ /*! \file vpImageConvert.h diff --git a/modules/core/include/visp3/core/vpImageDraw.h b/modules/core/include/visp3/core/vpImageDraw.h index 0b54430db4..88425623a6 100644 --- a/modules/core/include/visp3/core/vpImageDraw.h +++ b/modules/core/include/visp3/core/vpImageDraw.h @@ -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: * Drawing functions. - * -*****************************************************************************/ + */ #ifndef _vpImageDraw_h_ #define _vpImageDraw_h_ @@ -58,6 +56,7 @@ */ class VISP_EXPORT vpImageDraw { + public: static void drawArrow(vpImage &I, const vpImagePoint &ip1, const vpImagePoint &ip2, unsigned char color, unsigned int w = 4, unsigned int h = 2, unsigned int thickness = 1); diff --git a/modules/core/include/visp3/core/vpImageException.h b/modules/core/include/visp3/core/vpImageException.h index 34f6a0cefb..9770d62326 100644 --- a/modules/core/include/visp3/core/vpImageException.h +++ b/modules/core/include/visp3/core/vpImageException.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,59 +29,48 @@ * * Description: * Exceptions that can be emitted by the vpImage class and its derivatives. - * -*****************************************************************************/ + */ #ifndef _vpImageException_h_ #define _vpImageException_h_ -/* ------------------------------------------------------------------------- - */ -/* --- INCLUDE ------------------------------------------------------------- - */ -/* ------------------------------------------------------------------------- - */ - -/* \file vpImageException.h - \brief error that can be emitted by the vpImage class and its derivatives +/*! + * \file vpImageException.h + * \brief error that can be emitted by the vpImage class and its derivatives */ -/* Classes standards. */ #include #include -#include /* Classe std::ostream. */ -#include /* Classe string. */ +#include +#include -/* ------------------------------------------------------------------------- - */ -/* --- CLASS --------------------------------------------------------------- - */ -/* ------------------------------------------------------------------------- - */ /*! - - \class vpImageException - \ingroup group_core_debug - \brief Error that can be emitted by the vpImage class and its derivatives. + * \class vpImageException + * \ingroup group_core_debug + * \brief Error that can be emitted by the vpImage class and its derivatives. */ class VISP_EXPORT vpImageException : public vpException { public: /*! - \brief Lists the possible error than can be emitted while calling - vpImage member - */ - enum errorImageCodeEnum { - ioError, - noFileNameError, - notInitializedError, - incorrectInitializationError, - notInTheImage + * \brief Lists the possible error than can be emitted while calling + * vpImage member + */ + enum errorImageCodeEnum + { + ioError, //!< Image io error + noFileNameError, //!< Image file name error + notInitializedError, //!< Image not initialized + incorrectInitializationError, //!< Wrong image initialization + notInTheImage //!< Pixel not in the image }; public: + /*! + * Constructor. + */ vpImageException(int id, const char *format, ...) { this->code = id; @@ -91,7 +79,15 @@ class VISP_EXPORT vpImageException : public vpException setMessage(format, args); va_end(args); } + + /*! + * Constructor. + */ vpImageException(int id, const std::string &msg) : vpException(id, msg) { } + + /*! + * Constructor. + */ explicit vpImageException(int id) : vpException(id) { } }; diff --git a/modules/core/include/visp3/core/vpImageFilter.h b/modules/core/include/visp3/core/vpImageFilter.h index ec03c45e55..054fcba05b 100644 --- a/modules/core/include/visp3/core/vpImageFilter.h +++ b/modules/core/include/visp3/core/vpImageFilter.h @@ -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: * Various image tools, convolution, ... - * -*****************************************************************************/ + */ #ifndef _vpImageFilter_h_ #define _vpImageFilter_h_ diff --git a/modules/core/include/visp3/core/vpImageMorphology.h b/modules/core/include/visp3/core/vpImageMorphology.h index 4c9e025d7f..2fd2b6408b 100644 --- a/modules/core/include/visp3/core/vpImageMorphology.h +++ b/modules/core/include/visp3/core/vpImageMorphology.h @@ -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: * Morphology tools. - * -*****************************************************************************/ + */ #ifndef _vpImageMorphology_h_ #define _vpImageMorphology_h_ diff --git a/modules/core/include/visp3/core/vpImagePoint.h b/modules/core/include/visp3/core/vpImagePoint.h index b308257214..9618b273e4 100644 --- a/modules/core/include/visp3/core/vpImagePoint.h +++ b/modules/core/include/visp3/core/vpImagePoint.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * 2D point useful for image processing - * - * Authors: - * Julien Dufour - * -*****************************************************************************/ + */ #ifndef _vpImagePoint_h_ #define _vpImagePoint_h_ diff --git a/modules/core/include/visp3/core/vpImageTools.h b/modules/core/include/visp3/core/vpImageTools.h index 1505532e52..a9a12d081a 100644 --- a/modules/core/include/visp3/core/vpImageTools.h +++ b/modules/core/include/visp3/core/vpImageTools.h @@ -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: * Image tools. - * -*****************************************************************************/ + */ #ifndef vpImageTools_H #define vpImageTools_H diff --git a/modules/core/include/visp3/core/vpIoException.h b/modules/core/include/visp3/core/vpIoException.h index 2d0c202e70..55cf163360 100644 --- a/modules/core/include/visp3/core/vpIoException.h +++ b/modules/core/include/visp3/core/vpIoException.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,22 +29,14 @@ * * Description: * Exceptions that can be emitted by the vpIo class and its derivatives. - * -*****************************************************************************/ + */ #ifndef _vpIoException_h_ #define _vpIoException_h_ -/* ------------------------------------------------------------------------- - */ -/* --- INCLUDE ------------------------------------------------------------- - */ -/* ------------------------------------------------------------------------- - */ - /*! - \file vpIoException.h - \brief Error that can be emitted by the vpIoTools class and its derivatives. + * \file vpIoException.h + * \brief Error that can be emitted by the vpIoTools class and its derivatives. */ #include @@ -54,33 +45,30 @@ #include #include -/* ------------------------------------------------------------------------- - */ -/* --- CLASS --------------------------------------------------------------- - */ -/* ------------------------------------------------------------------------- - */ - /*! - \class vpIoException - \ingroup group_core_debug - \brief Error that can be emitted by the vpIoTools class and its derivatives. + * \class vpIoException + * \ingroup group_core_debug + * \brief Error that can be emitted by the vpIoTools class and its derivatives. */ class VISP_EXPORT vpIoException : public vpException { public: /*! - \brief Lists the possible error than can be emitted while calling - vpIo member. - */ - enum error { - invalidDirectoryName, /*! Directory name is invalid. */ - cantCreateDirectory, /*! Unable to create a directory. */ - cantGetUserName, /*! User name is not available. */ - cantGetenv /*! Cannot get environment variable value. */ + * \brief Lists the possible error than can be emitted while calling + * vpIo member. + */ + enum error + { + invalidDirectoryName, //!< Directory name is invalid. + cantCreateDirectory, //!< Unable to create a directory. + cantGetUserName, //!< User name is not available. + cantGetenv //!< Cannot get environment variable value. }; public: + /*! + * Constructor. + */ vpIoException(int id, const char *format, ...) { this->code = id; @@ -89,7 +77,15 @@ class VISP_EXPORT vpIoException : public vpException setMessage(format, args); va_end(args); } + + /*! + * Constructor. + */ vpIoException(int id, const std::string &msg) : vpException(id, msg) { } + + /*! + * Constructor. + */ explicit vpIoException(int id) : vpException(id) { } }; diff --git a/modules/core/include/visp3/core/vpIoTools.h b/modules/core/include/visp3/core/vpIoTools.h index 3e88c230e4..6854533379 100644 --- a/modules/core/include/visp3/core/vpIoTools.h +++ b/modules/core/include/visp3/core/vpIoTools.h @@ -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: * Directory management. - * -*****************************************************************************/ + */ #ifndef _vpIoTools_h_ #define _vpIoTools_h_ diff --git a/modules/core/include/visp3/core/vpJsonParsing.h b/modules/core/include/visp3/core/vpJsonParsing.h index daf106df9b..967cdee2e2 100644 --- a/modules/core/include/visp3/core/vpJsonParsing.h +++ b/modules/core/include/visp3/core/vpJsonParsing.h @@ -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: * JSON parsing helpers. - * -*****************************************************************************/ + */ #ifndef _vpJsonParsing_h_ #define _vpJsonParsing_h_ diff --git a/modules/core/include/visp3/core/vpKalmanFilter.h b/modules/core/include/visp3/core/vpKalmanFilter.h index ccc704cf43..b9461884ec 100644 --- a/modules/core/include/visp3/core/vpKalmanFilter.h +++ b/modules/core/include/visp3/core/vpKalmanFilter.h @@ -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: * Kalman filtering. - * -*****************************************************************************/ + */ #ifndef vpKalmanFilter_h #define vpKalmanFilter_h diff --git a/modules/core/include/visp3/core/vpLinProg.h b/modules/core/include/visp3/core/vpLinProg.h index 8b58d15a77..38586d341b 100644 --- a/modules/core/include/visp3/core/vpLinProg.h +++ b/modules/core/include/visp3/core/vpLinProg.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Linear Programming with simplex - * - * Authors: - * Olivier Kermorgant - * -*****************************************************************************/ + */ #ifndef vpLinProgh #define vpLinProgh diff --git a/modules/core/include/visp3/core/vpLine.h b/modules/core/include/visp3/core/vpLine.h index 7c305203df..83ae2471b2 100644 --- a/modules/core/include/visp3/core/vpLine.h +++ b/modules/core/include/visp3/core/vpLine.h @@ -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: * Line feature. - * -*****************************************************************************/ + */ #ifndef vpLine_H #define vpLine_H diff --git a/modules/core/include/visp3/core/vpLinearKalmanFilterInstantiation.h b/modules/core/include/visp3/core/vpLinearKalmanFilterInstantiation.h index dee0cdf513..54e39faa6c 100644 --- a/modules/core/include/visp3/core/vpLinearKalmanFilterInstantiation.h +++ b/modules/core/include/visp3/core/vpLinearKalmanFilterInstantiation.h @@ -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: * Kalman filtering. - * -*****************************************************************************/ + */ #ifndef vpLinearKalmanFilterInstantiation_h #define vpLinearKalmanFilterInstantiation_h diff --git a/modules/core/include/visp3/core/vpList.h b/modules/core/include/visp3/core/vpList.h index a74b6e362b..f1cd034b63 100644 --- a/modules/core/include/visp3/core/vpList.h +++ b/modules/core/include/visp3/core/vpList.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * List data structure. - * - * Authors: - * Nicolas Mansard : Toward const-specification respect - * -*****************************************************************************/ + */ #ifndef VP_LIST_H #define VP_LIST_H diff --git a/modules/core/include/visp3/core/vpMath.h b/modules/core/include/visp3/core/vpMath.h index 16cd30c5f2..f082ab5d33 100644 --- a/modules/core/include/visp3/core/vpMath.h +++ b/modules/core/include/visp3/core/vpMath.h @@ -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: * Simple mathematical function not available in the C math library (math.h). - * -*****************************************************************************/ + */ /*! \file vpMath.h diff --git a/modules/core/include/visp3/core/vpMatrix.h b/modules/core/include/visp3/core/vpMatrix.h index b334fc6f9f..2b6b65bdf6 100644 --- a/modules/core/include/visp3/core/vpMatrix.h +++ b/modules/core/include/visp3/core/vpMatrix.h @@ -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: * Matrix manipulation. - * -*****************************************************************************/ + */ #ifndef vpMatrix_H #define vpMatrix_H diff --git a/modules/core/include/visp3/core/vpMatrixException.h b/modules/core/include/visp3/core/vpMatrixException.h index bf237a4ada..c38ee9a49a 100644 --- a/modules/core/include/visp3/core/vpMatrixException.h +++ b/modules/core/include/visp3/core/vpMatrixException.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,66 +29,57 @@ * * Description: * Exceptions that can be emitted by the vpMatrix class and its derivatives. - * -*****************************************************************************/ + */ #ifndef _vpMatrixException_h_ #define _vpMatrixException_h_ -/* ------------------------------------------------------------------------- - */ -/* --- INCLUDE ------------------------------------------------------------- - */ -/* ------------------------------------------------------------------------- - */ - -/* Classes standards. */ -// - #include #include -#include /* Classe std::ostream. */ -#include /* Classe string. */ - -/* ------------------------------------------------------------------------- - */ -/* --- CLASS --------------------------------------------------------------- - */ -/* ------------------------------------------------------------------------- - */ +#include +#include /*! - \class vpMatrixException - \ingroup group_core_debug - \brief error that can be emitted by the vpMatrix class and its derivatives + * \class vpMatrixException + * \ingroup group_core_debug + * \brief error that can be emitted by the vpMatrix class and its derivatives */ class VISP_EXPORT vpMatrixException : public vpException { public: /*! - \brief Lists the possible error than can be emitted while calling - vpMatrix member - */ - enum errorCodeEnum { - //! error returns by a constructor + * \brief Lists the possible error than can be emitted while calling + * vpMatrix member + */ + enum errorCodeEnum + { +//! Error returns by a constructor constructionError, - //! something is not initialized + //! Something is not initialized notInitializedError, - //! function not implemented + //! Function not implemented notImplementedError, - //! index out of range + //! Index out of range outOfRangeError, - //! iterative algorithm doesn't converge (ex SVD) + //! Iterative algorithm doesn't converge (ex SVD) convergencyError, + //! Incorrect matrix size incorrectMatrixSizeError, + //! Forbidden operation forbiddenOperatorError, + //! Sub operation matrix error subMatrixError, + //! Matrix operation error matrixError, + //! Rank deficient rankDeficient }; public: + /*! + * Constructor. + */ vpMatrixException(int id, const char *format, ...) { this->code = id; @@ -98,9 +88,16 @@ class VISP_EXPORT vpMatrixException : public vpException setMessage(format, args); va_end(args); } + + /*! + * Constructor. + */ vpMatrixException(int id, const std::string &msg) : vpException(id, msg) { } + + /*! + * Constructor. + */ explicit vpMatrixException(int id) : vpException(id) { } - // vpMatrixException() : vpException() { ;} }; #endif diff --git a/modules/core/include/visp3/core/vpMeterPixelConversion.h b/modules/core/include/visp3/core/vpMeterPixelConversion.h index 40a6bc4cb7..29c3f969ed 100644 --- a/modules/core/include/visp3/core/vpMeterPixelConversion.h +++ b/modules/core/include/visp3/core/vpMeterPixelConversion.h @@ -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: * Meter to pixel conversion. - * -*****************************************************************************/ + */ #ifndef _vpMeterPixelConversion_h_ #define _vpMeterPixelConversion_h_ diff --git a/modules/core/include/visp3/core/vpMoment.h b/modules/core/include/visp3/core/vpMoment.h index 3319e2547c..1074561c3b 100644 --- a/modules/core/include/visp3/core/vpMoment.h +++ b/modules/core/include/visp3/core/vpMoment.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Base for 2D moment descriptor - * - * Authors: - * Filip Novotny - * -*****************************************************************************/ + */ /*! \file vpMoment.h diff --git a/modules/core/include/visp3/core/vpMomentAlpha.h b/modules/core/include/visp3/core/vpMomentAlpha.h index 2ff967d4b8..6c52aeed00 100644 --- a/modules/core/include/visp3/core/vpMomentAlpha.h +++ b/modules/core/include/visp3/core/vpMomentAlpha.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Alpha moment descriptor for in-plane orientation. - * - * Authors: - * Filip Novotny - * -*****************************************************************************/ + */ /*! \file vpMomentAlpha.h @@ -178,7 +173,7 @@ This program outputs: \code --- Reference object --- alphaRef=25.3019 deg -mu3=1.80552 0.921882 0.385828 0.122449 +mu3=1.80552 0.921882 0.385828 0.122449 --- current object --- alpha=-25.3019 deg \endcode diff --git a/modules/core/include/visp3/core/vpMomentArea.h b/modules/core/include/visp3/core/vpMomentArea.h index d47c498f1e..d9467c12e4 100644 --- a/modules/core/include/visp3/core/vpMomentArea.h +++ b/modules/core/include/visp3/core/vpMomentArea.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Just the area m00 = mu00 - * - * Authors: - * Manikandan Bakthavatchalam - * -*****************************************************************************/ + */ #ifndef _vpMomentArea_h_ #define _vpMomentArea_h_ diff --git a/modules/core/include/visp3/core/vpMomentAreaNormalized.h b/modules/core/include/visp3/core/vpMomentAreaNormalized.h index 6fd8beadbb..ab512fd9d7 100644 --- a/modules/core/include/visp3/core/vpMomentAreaNormalized.h +++ b/modules/core/include/visp3/core/vpMomentAreaNormalized.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * 2D normalized surface moment descriptor (usually described as An) - * - * Authors: - * Filip Novotny - * -*****************************************************************************/ + */ /*! \file vpMomentAreaNormalized.h \brief 2D normalized surface moment descriptor (usually described as An). diff --git a/modules/core/include/visp3/core/vpMomentBasic.h b/modules/core/include/visp3/core/vpMomentBasic.h index a11694ede0..5d8acf8dc1 100644 --- a/modules/core/include/visp3/core/vpMomentBasic.h +++ b/modules/core/include/visp3/core/vpMomentBasic.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Basic moment descriptor - * - * Authors: - * Filip Novotny - * -*****************************************************************************/ + */ /*! \file vpMomentBasic.h diff --git a/modules/core/include/visp3/core/vpMomentCInvariant.h b/modules/core/include/visp3/core/vpMomentCInvariant.h index fdd3b545bc..6323fa859f 100644 --- a/modules/core/include/visp3/core/vpMomentCInvariant.h +++ b/modules/core/include/visp3/core/vpMomentCInvariant.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -29,16 +28,12 @@ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. * * Description: - * Descriptor for various invariants used to drive space roations around X and - *Y axis. - * - * Authors: - * Filip Novotny - * -*****************************************************************************/ + * Descriptor for various invariants used to drive space rotations around X and + * Y axis. + */ /*! \file vpMomentCInvariant.h - \brief Descriptor for various invariants used to drive space roations around + \brief Descriptor for various invariants used to drive space rotations around X and Y axis. */ #ifndef _vpMomentCInvariant_h_ @@ -145,7 +140,7 @@ class VISP_EXPORT vpMomentCInvariant : public vpMoment public: explicit vpMomentCInvariant(bool flg_sxsynormalization = false); - virtual ~vpMomentCInvariant(){}; + virtual ~vpMomentCInvariant() { }; /*! Shorcut for getting the value of \f$C_1\f$. diff --git a/modules/core/include/visp3/core/vpMomentCentered.h b/modules/core/include/visp3/core/vpMomentCentered.h index 885d5723b0..2e0a8043a9 100644 --- a/modules/core/include/visp3/core/vpMomentCentered.h +++ b/modules/core/include/visp3/core/vpMomentCentered.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Centered moment descriptor - * - * Authors: - * Filip Novotny - * -*****************************************************************************/ + */ #ifndef _vpMomentCentered_h_ #define _vpMomentCentered_h_ diff --git a/modules/core/include/visp3/core/vpMomentCommon.h b/modules/core/include/visp3/core/vpMomentCommon.h index b95fd05212..5fbae91c2d 100644 --- a/modules/core/include/visp3/core/vpMomentCommon.h +++ b/modules/core/include/visp3/core/vpMomentCommon.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Pre-filled moment database with all commonly used moments. - * - * Authors: - * Filip Novotny - * -*****************************************************************************/ + */ /*! \file vpMomentCommon.h \brief Pre-filled moment database with all commonly used moments. diff --git a/modules/core/include/visp3/core/vpMomentDatabase.h b/modules/core/include/visp3/core/vpMomentDatabase.h index 84ddc1eed9..f9b4c80902 100644 --- a/modules/core/include/visp3/core/vpMomentDatabase.h +++ b/modules/core/include/visp3/core/vpMomentDatabase.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Pseudo-database used to handle dependencies between moments - * - * Authors: - * Filip Novotny - * -*****************************************************************************/ + */ /*! \file vpMomentDatabase.h \brief Pseudo-database used to handle dependencies between moments. @@ -119,8 +114,8 @@ int main() Gravity center: Xg=1.5, Yg=1.5 Centered moments: -2 0 -0 x +2 0 +0 x \endcode A moment is identified in the database by it's vpMoment::name method. @@ -132,11 +127,11 @@ class VISP_EXPORT vpMomentDatabase { private: #ifndef DOXYGEN_SHOULD_SKIP_THIS - struct cmp_str { + struct vpCmpStr_t { bool operator()(char const *a, char const *b) const { return std::strcmp(a, b) < 0; } }; #endif - std::map moments; + std::map moments; void add(vpMoment &moment, const char *name); public: diff --git a/modules/core/include/visp3/core/vpMomentGravityCenter.h b/modules/core/include/visp3/core/vpMomentGravityCenter.h index a421bb27b5..66e212ca76 100644 --- a/modules/core/include/visp3/core/vpMomentGravityCenter.h +++ b/modules/core/include/visp3/core/vpMomentGravityCenter.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * 2D Gravity Center moment descriptor (usually described by the pair Xg,Yg) - * - * Authors: - * Filip Novotny - * -*****************************************************************************/ + */ /*! \file vpMomentGravityCenter.h \brief 2D Gravity Center moment descriptor (usually described by the pair diff --git a/modules/core/include/visp3/core/vpMomentGravityCenterNormalized.h b/modules/core/include/visp3/core/vpMomentGravityCenterNormalized.h index 7b03d9162a..a86e80af28 100644 --- a/modules/core/include/visp3/core/vpMomentGravityCenterNormalized.h +++ b/modules/core/include/visp3/core/vpMomentGravityCenterNormalized.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,12 +29,8 @@ * * Description: * 2D normalized gravity center moment descriptor (usually described by the - *pair Xn,Yn) - * - * Authors: - * Filip Novotny - * -*****************************************************************************/ + * pair Xn,Yn) + */ /*! \file vpMomentGravityCenterNormalized.h \brief 2D normalized gravity center moment descriptor (usually described by diff --git a/modules/core/include/visp3/core/vpMomentObject.h b/modules/core/include/visp3/core/vpMomentObject.h index dda4377171..1e39cec69d 100644 --- a/modules/core/include/visp3/core/vpMomentObject.h +++ b/modules/core/include/visp3/core/vpMomentObject.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Object input structure used by moments. - * - * Authors: - * Filip Novotny - * Manikandan Bakthavatchalam - *****************************************************************************/ + */ /*! \file vpMomentObject.h \brief Object input structure used by moments. @@ -194,12 +189,12 @@ m04=0.00080625 m14=-7.125e-05 m05=-6.59375e-05 Basic moment available: -4 0.1 0.21 0.019 0.0129 0.00211 --0.05 0.02 0.003 0.0023 0.00057 x -0.0525 -0.0015 0.0026 9e-05 x x --0.002375 0.000575 -4.5e-05 x x x -0.00080625 -7.125e-05 x x x x --6.59375e-05 x x x x x +4 0.1 0.21 0.019 0.0129 0.00211 +-0.05 0.02 0.003 0.0023 0.00057 x +0.0525 -0.0015 0.0026 9e-05 x x +-0.002375 0.000575 -4.5e-05 x x x +0.00080625 -7.125e-05 x x x x +-6.59375e-05 x x x x x Direct access to some basic moments: m00: 4 @@ -225,7 +220,8 @@ class VISP_EXPORT vpMomentObject /*! Type of object that will be considered. */ - typedef enum { + typedef enum + { DENSE_FULL_OBJECT = 0, /*!< A set of points (typically from an image) which are interpreted as being dense. */ DENSE_POLYGON = 1, /*!< A set of points (stored in clockwise order) @@ -237,7 +233,8 @@ class VISP_EXPORT vpMomentObject /*! Type of camera image background. */ - typedef enum { + typedef enum + { BLACK = 0, //!< Black background WHITE = 1, //!< Not functional right now } vpCameraImgBckGrndType; diff --git a/modules/core/include/visp3/core/vpMouseButton.h b/modules/core/include/visp3/core/vpMouseButton.h index 16812388f2..3d68601f20 100644 --- a/modules/core/include/visp3/core/vpMouseButton.h +++ b/modules/core/include/visp3/core/vpMouseButton.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,9 +29,7 @@ * * Description: * Color definition. - * - * -*****************************************************************************/ + */ #ifndef vpMouseButton_h #define vpMouseButton_h diff --git a/modules/core/include/visp3/core/vpMunkres.h b/modules/core/include/visp3/core/vpMunkres.h index 1228f9d318..11db82c4d5 100644 --- a/modules/core/include/visp3/core/vpMunkres.h +++ b/modules/core/include/visp3/core/vpMunkres.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,12 +29,7 @@ * * Description: * Class for Munkres Assignment Algorithm. - * - * Authors: - * Souriya Trinh - * Julien Dufour - * -*****************************************************************************/ + */ #pragma once @@ -44,7 +38,7 @@ #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \ (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911))) -// Visual Studio: Optionals are available from Visual Studio 2017 RTW (15.0) [1910] +// Visual Studio: Optionals are available from Visual Studio 2017 RTW (15.0) [1910] // Visual Studio: Structured bindings are available from Visual Studio 2017 version 15.3 [1911] // System diff --git a/modules/core/include/visp3/core/vpMutex.h b/modules/core/include/visp3/core/vpMutex.h index 4faeff7944..96a8fc0501 100644 --- a/modules/core/include/visp3/core/vpMutex.h +++ b/modules/core/include/visp3/core/vpMutex.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Mutex protection. - * - * Authors: - * Celine Teuliere - * -*****************************************************************************/ + */ #ifndef _vpMutex_h_ #define _vpMutex_h_ diff --git a/modules/core/include/visp3/core/vpNetwork.h b/modules/core/include/visp3/core/vpNetwork.h index e1f2427f8f..0daa9a5173 100644 --- a/modules/core/include/visp3/core/vpNetwork.h +++ b/modules/core/include/visp3/core/vpNetwork.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * TCP Network - * - * Authors: - * Aurelien Yol - * -*****************************************************************************/ + */ #ifndef vpNetwork_H #define vpNetwork_H @@ -155,13 +150,13 @@ class VISP_EXPORT vpNetwork bool verboseMode; private: - std::vector _handleRequests(); - int _handleFirstRequest(); + std::vector privHandleRequests(); + int privHandleFirstRequest(); - void _receiveRequest(); - void _receiveRequestFrom(const unsigned int &receptorEmitting); - int _receiveRequestOnce(); - int _receiveRequestOnceFrom(const unsigned int &receptorEmitting); + void privReceiveRequest(); + void privReceiveRequestFrom(const unsigned int &receptorEmitting); + int privReceiveRequestOnce(); + int privReceiveRequestOnceFrom(const unsigned int &receptorEmitting); public: vpNetwork(); diff --git a/modules/core/include/visp3/core/vpNoise.h b/modules/core/include/visp3/core/vpNoise.h index 06c61bfa71..eac6c7b780 100644 --- a/modules/core/include/visp3/core/vpNoise.h +++ b/modules/core/include/visp3/core/vpNoise.h @@ -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: * Generation of random number with uniform and normal probability density. - * -*****************************************************************************/ + */ #ifndef vpNoise_hh #define vpNoise_hh diff --git a/modules/core/include/visp3/core/vpPixelMeterConversion.h b/modules/core/include/visp3/core/vpPixelMeterConversion.h index a9721431c2..0e546b1149 100644 --- a/modules/core/include/visp3/core/vpPixelMeterConversion.h +++ b/modules/core/include/visp3/core/vpPixelMeterConversion.h @@ -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: * Pixel to meter conversion. - * -*****************************************************************************/ + */ #ifndef _vpPixelMeterConversion_h_ #define _vpPixelMeterConversion_h_ diff --git a/modules/core/include/visp3/core/vpPlane.h b/modules/core/include/visp3/core/vpPlane.h index d37cdf0d68..7c150a641f 100644 --- a/modules/core/include/visp3/core/vpPlane.h +++ b/modules/core/include/visp3/core/vpPlane.h @@ -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: * Plane geometrical structure. - * -*****************************************************************************/ + */ #ifndef vpPlane_hh #define vpPlane_hh diff --git a/modules/core/include/visp3/core/vpPoint.h b/modules/core/include/visp3/core/vpPoint.h index a16eb1cb25..989859132d 100644 --- a/modules/core/include/visp3/core/vpPoint.h +++ b/modules/core/include/visp3/core/vpPoint.h @@ -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: * Point feature. - * -*****************************************************************************/ + */ #ifndef vpPoint_H #define vpPoint_H diff --git a/modules/core/include/visp3/core/vpPolygon.h b/modules/core/include/visp3/core/vpPolygon.h index b62a23a7b8..4084ccf85f 100644 --- a/modules/core/include/visp3/core/vpPolygon.h +++ b/modules/core/include/visp3/core/vpPolygon.h @@ -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: * Defines a generic 2D polygon. - * -*****************************************************************************/ + */ #ifndef _vpPolygon_h_ #define _vpPolygon_h_ diff --git a/modules/core/include/visp3/core/vpPolygon3D.h b/modules/core/include/visp3/core/vpPolygon3D.h index fd73a97686..cb3e5b3bb2 100644 --- a/modules/core/include/visp3/core/vpPolygon3D.h +++ b/modules/core/include/visp3/core/vpPolygon3D.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Implements a polygon of the model used by the model-based tracker. - * - * Authors: - * Aurelien Yol - * -*****************************************************************************/ + */ /*! \file vpPolygon3D.h diff --git a/modules/core/include/visp3/core/vpPoseVector.h b/modules/core/include/visp3/core/vpPoseVector.h index e0b79d2f16..e98544a936 100644 --- a/modules/core/include/visp3/core/vpPoseVector.h +++ b/modules/core/include/visp3/core/vpPoseVector.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -31,8 +30,7 @@ * Description: * Pose object. A pose is a size 6 vector [t, tu]^T where tu is * a rotation vector (theta u representation) and t is a translation vector. - * -*****************************************************************************/ + */ #ifndef vpPOSEVECTOR_H #define vpPOSEVECTOR_H diff --git a/modules/core/include/visp3/core/vpQuadProg.h b/modules/core/include/visp3/core/vpQuadProg.h index 72949a4ae6..e63d638631 100644 --- a/modules/core/include/visp3/core/vpQuadProg.h +++ b/modules/core/include/visp3/core/vpQuadProg.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Quadratic Programming - * - * Authors: - * Olivier Kermorgant - * -*****************************************************************************/ + */ #ifndef vpQuadProgh #define vpQuadProgh diff --git a/modules/core/include/visp3/core/vpQuaternionVector.h b/modules/core/include/visp3/core/vpQuaternionVector.h index 7f8d3393f6..7b4d177535 100644 --- a/modules/core/include/visp3/core/vpQuaternionVector.h +++ b/modules/core/include/visp3/core/vpQuaternionVector.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Quaternion definition. - * - * Authors: - * Filip Novotny - * -*****************************************************************************/ + */ #ifndef _vpQuaternionVector_h_ #define _vpQuaternionVector_h_ diff --git a/modules/core/include/visp3/core/vpRGBa.h b/modules/core/include/visp3/core/vpRGBa.h index 8cd8d265b3..f44bf0e735 100644 --- a/modules/core/include/visp3/core/vpRGBa.h +++ b/modules/core/include/visp3/core/vpRGBa.h @@ -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: * RGBA pixel. - * -*****************************************************************************/ + */ #ifndef vpRGBa_h #define vpRGBa_h diff --git a/modules/core/include/visp3/core/vpRGBf.h b/modules/core/include/visp3/core/vpRGBf.h index 5e5a75d702..1db9f4b63e 100644 --- a/modules/core/include/visp3/core/vpRGBf.h +++ b/modules/core/include/visp3/core/vpRGBf.h @@ -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: * 32-bit floating point RGB pixel. - * -*****************************************************************************/ + */ #ifndef vpRGBf_h #define vpRGBf_h diff --git a/modules/core/include/visp3/core/vpRansac.h b/modules/core/include/visp3/core/vpRansac.h index 3397bb366b..fedd345f98 100644 --- a/modules/core/include/visp3/core/vpRansac.h +++ b/modules/core/include/visp3/core/vpRansac.h @@ -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: * Ransac robust algorithm. - * -*****************************************************************************/ + */ /*! \file vpRansac.h diff --git a/modules/core/include/visp3/core/vpRect.h b/modules/core/include/visp3/core/vpRect.h index 6154f70c57..985d0dbd73 100644 --- a/modules/core/include/visp3/core/vpRect.h +++ b/modules/core/include/visp3/core/vpRect.h @@ -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: * Defines a rectangle in the plane. - * -*****************************************************************************/ + */ #ifndef vpRect_h #define vpRect_h diff --git a/modules/core/include/visp3/core/vpRectOriented.h b/modules/core/include/visp3/core/vpRectOriented.h index 2795f60cb5..bfdd116a07 100644 --- a/modules/core/include/visp3/core/vpRectOriented.h +++ b/modules/core/include/visp3/core/vpRectOriented.h @@ -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: * Defines a (possibly oriented) rectangle in the plane. - * -*****************************************************************************/ + */ #ifndef vpRectOriented_h #define vpRectOriented_h diff --git a/modules/core/include/visp3/core/vpRequest.h b/modules/core/include/visp3/core/vpRequest.h index efec29ada0..c2370de1d6 100644 --- a/modules/core/include/visp3/core/vpRequest.h +++ b/modules/core/include/visp3/core/vpRequest.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Network Request. - * - * Authors: - * Aurelien Yol - * -*****************************************************************************/ + */ #ifndef vpRequest_H #define vpRequest_H diff --git a/modules/core/include/visp3/core/vpRobust.h b/modules/core/include/visp3/core/vpRobust.h index 54ed46af0d..0746e9a23c 100644 --- a/modules/core/include/visp3/core/vpRobust.h +++ b/modules/core/include/visp3/core/vpRobust.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,12 +29,7 @@ * * Description: * M-Estimator and various influence function. - * - * Authors: - * Andrew Comport - * Jean Laneurit - * -*****************************************************************************/ + */ /*! \file vpRobust.h diff --git a/modules/core/include/visp3/core/vpRotationMatrix.h b/modules/core/include/visp3/core/vpRotationMatrix.h index 1d9a9a76cb..0862fa8cbb 100644 --- a/modules/core/include/visp3/core/vpRotationMatrix.h +++ b/modules/core/include/visp3/core/vpRotationMatrix.h @@ -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: * Rotation matrix. - * -*****************************************************************************/ + */ #ifndef _vpRotationMatrix_h_ #define _vpRotationMatrix_h_ diff --git a/modules/core/include/visp3/core/vpRotationVector.h b/modules/core/include/visp3/core/vpRotationVector.h index 9b580c47a4..3ded305a11 100644 --- a/modules/core/include/visp3/core/vpRotationVector.h +++ b/modules/core/include/visp3/core/vpRotationVector.h @@ -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: * Generic rotation vector (cannot be used as is !). - * -*****************************************************************************/ + */ #ifndef _vpRotationVector_h_ #define _vpRotationVector_h_ diff --git a/modules/core/include/visp3/core/vpRowVector.h b/modules/core/include/visp3/core/vpRowVector.h index 7a2164771d..2e17c4ea18 100644 --- a/modules/core/include/visp3/core/vpRowVector.h +++ b/modules/core/include/visp3/core/vpRowVector.h @@ -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: * Operation on row vectors. - * -*****************************************************************************/ + */ #ifndef vpRowVector_H #define vpRowVector_H diff --git a/modules/core/include/visp3/core/vpRxyzVector.h b/modules/core/include/visp3/core/vpRxyzVector.h index c5ecf7a073..330807fcec 100644 --- a/modules/core/include/visp3/core/vpRxyzVector.h +++ b/modules/core/include/visp3/core/vpRxyzVector.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -31,8 +30,7 @@ * Description: * Rxyz angle parameterization for the rotation. * Rxyz(phi,theta,psi) = Rot(x,phi)Rot(y,theta)Rot(z,psi). - * -*****************************************************************************/ + */ #ifndef _vpRxyzVector_h_ #define _vpRxyzVector_h_ diff --git a/modules/core/include/visp3/core/vpRzyxVector.h b/modules/core/include/visp3/core/vpRzyxVector.h index b68f68fab9..fce560b5d1 100644 --- a/modules/core/include/visp3/core/vpRzyxVector.h +++ b/modules/core/include/visp3/core/vpRzyxVector.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -31,8 +30,7 @@ * Description: * Rzyx angle parameterization for the rotation. * Rzyx(phi,theta,psi) = Rot(z,phi)Rot(y,theta)Rot(x,psi) - * -*****************************************************************************/ + */ #ifndef _vpRzyxVector_h_ #define _vpRzyxVector_h_ diff --git a/modules/core/include/visp3/core/vpRzyzVector.h b/modules/core/include/visp3/core/vpRzyzVector.h index 3c763a78c4..fce5874764 100644 --- a/modules/core/include/visp3/core/vpRzyzVector.h +++ b/modules/core/include/visp3/core/vpRzyzVector.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -31,8 +30,7 @@ * Description: * Euler angles parameterization for the rotation. * Rzyz(phi,theta,psi) = Rot(z,phi)Rot(y,theta)Rot(z,psi) - * -*****************************************************************************/ + */ #ifndef _vpRzyzVector_h_ #define _vpRzyzVector_h_ diff --git a/modules/core/include/visp3/core/vpScale.h b/modules/core/include/visp3/core/vpScale.h index 6d19b1aabb..98da42ad49 100644 --- a/modules/core/include/visp3/core/vpScale.h +++ b/modules/core/include/visp3/core/vpScale.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,12 +29,8 @@ * * Description: * Median Absolute Deviation (MAD), MPDE, Mean shift kernel density - *estimation. - * - * Authors: - * Andrew Comport - * -*****************************************************************************/ + * estimation. + */ /*! \file vpScale.h @@ -45,7 +40,7 @@ /*! * \brief Contains various estimators for scale. * \n Methods : Median Absolute Deviation (MAD), - * MPDE, Mean shift kernel + * MPDE, Mean shift kernel * density estimation. \author Andrew Comport \date 24/10/03 */ // ========================================================== diff --git a/modules/core/include/visp3/core/vpSerial.h b/modules/core/include/visp3/core/vpSerial.h index 3cc15717d8..1707e9c619 100644 --- a/modules/core/include/visp3/core/vpSerial.h +++ b/modules/core/include/visp3/core/vpSerial.h @@ -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: * Serial communication. - * -*****************************************************************************/ + */ #ifndef _vpSerial_h_ #define _vpSerial_h_ diff --git a/modules/core/include/visp3/core/vpServer.h b/modules/core/include/visp3/core/vpServer.h index 3a6748845b..d5eac7597c 100644 --- a/modules/core/include/visp3/core/vpServer.h +++ b/modules/core/include/visp3/core/vpServer.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * TCP Server - * - * Authors: - * Aurelien Yol - * -*****************************************************************************/ + */ #ifndef vpServer_H #define vpServer_H diff --git a/modules/core/include/visp3/core/vpSphere.h b/modules/core/include/visp3/core/vpSphere.h index 8f39251cf9..4c818f7c5e 100644 --- a/modules/core/include/visp3/core/vpSphere.h +++ b/modules/core/include/visp3/core/vpSphere.h @@ -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: * Sphere feature. - * -*****************************************************************************/ + */ /*! \file vpSphere.h diff --git a/modules/core/include/visp3/core/vpSubColVector.h b/modules/core/include/visp3/core/vpSubColVector.h index 21aa5059c7..58f9ec9d79 100644 --- a/modules/core/include/visp3/core/vpSubColVector.h +++ b/modules/core/include/visp3/core/vpSubColVector.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -29,12 +28,8 @@ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. * * Description: - * Mask on a vpColVector . - * - * Authors: - * Laneurit Jean - * -*****************************************************************************/ + * Mask on a vpColVector. + */ #ifndef _vpSubColVector_h_ #define _vpSubColVector_h_ diff --git a/modules/core/include/visp3/core/vpSubMatrix.h b/modules/core/include/visp3/core/vpSubMatrix.h index e50e398247..8cabf02d03 100644 --- a/modules/core/include/visp3/core/vpSubMatrix.h +++ b/modules/core/include/visp3/core/vpSubMatrix.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -29,12 +28,8 @@ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. * * Description: - * Mask on a vpMatrix . - * - * Authors: - * Laneurit Jean - * -*****************************************************************************/ + * Mask on a vpMatrix. + */ #ifndef _vpSubMatrix_h_ #define _vpSubMatrix_h_ diff --git a/modules/core/include/visp3/core/vpSubRowVector.h b/modules/core/include/visp3/core/vpSubRowVector.h index 1af4c75023..21660d7cf5 100644 --- a/modules/core/include/visp3/core/vpSubRowVector.h +++ b/modules/core/include/visp3/core/vpSubRowVector.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -29,12 +28,8 @@ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. * * Description: - * Mask on a vpRowVector . - * - * Authors: - * Laneurit Jean - * -*****************************************************************************/ + * Mask on a vpRowVector. + */ #ifndef _vpSubRowVector_h_ #define _vpSubRowVector_h_ diff --git a/modules/core/include/visp3/core/vpThetaUVector.h b/modules/core/include/visp3/core/vpThetaUVector.h index 6487691a0c..b1c6d10dc9 100644 --- a/modules/core/include/visp3/core/vpThetaUVector.h +++ b/modules/core/include/visp3/core/vpThetaUVector.h @@ -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: * Theta U parameterization for the rotation. - * -*****************************************************************************/ + */ #ifndef _vpThetaUVector_h_ #define _vpThetaUVector_h_ diff --git a/modules/core/include/visp3/core/vpThread.h b/modules/core/include/visp3/core/vpThread.h index d59708467c..f795aca60f 100644 --- a/modules/core/include/visp3/core/vpThread.h +++ b/modules/core/include/visp3/core/vpThread.h @@ -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: * Threading capabilities - * -*****************************************************************************/ + */ #ifndef _vpPthread_h_ #define _vpPthread_h_ diff --git a/modules/core/include/visp3/core/vpTime.h b/modules/core/include/visp3/core/vpTime.h index 9ea5df19d2..3bc5d5ae2b 100644 --- a/modules/core/include/visp3/core/vpTime.h +++ b/modules/core/include/visp3/core/vpTime.h @@ -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: * Time management and measurement. - * -*****************************************************************************/ + */ #ifndef vpTime_h #define vpTime_h diff --git a/modules/core/include/visp3/core/vpTracker.h b/modules/core/include/visp3/core/vpTracker.h index a227d70b42..25662d0f22 100644 --- a/modules/core/include/visp3/core/vpTracker.h +++ b/modules/core/include/visp3/core/vpTracker.h @@ -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: * Generic tracker. - * -*****************************************************************************/ + */ #ifndef vpTracker_H #define vpTracker_H diff --git a/modules/core/include/visp3/core/vpTrackingException.h b/modules/core/include/visp3/core/vpTrackingException.h index d2c090b0bf..c17e6bcd9a 100644 --- a/modules/core/include/visp3/core/vpTrackingException.h +++ b/modules/core/include/visp3/core/vpTrackingException.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,56 +29,45 @@ * * Description: * Exceptions that can be emitted by the vpTracking class and its derivatives. - * -*****************************************************************************/ + */ #ifndef _vpTrackingException_H #define _vpTrackingException_H -/* ------------------------------------------------------------------------- - */ -/* --- INCLUDE ------------------------------------------------------------- - */ -/* ------------------------------------------------------------------------- - */ - -/* \file vpTrackingException.h - \brief error that can be emitted by the vpTracker class and its derivatives +/*! + * \file vpTrackingException.h + * \brief error that can be emitted by the vpTracker class and its derivatives */ -/* Classes standards. */ -#include /* Classe std::ostream. */ -#include /* Classe string. */ +#include +#include #include -/* ------------------------------------------------------------------------- - */ -/* --- CLASS --------------------------------------------------------------- - */ -/* ------------------------------------------------------------------------- - */ /*! - \class vpTrackingException - \ingroup group_core_debug - \brief Error that can be emitted by the vpTracker class and its derivatives. + * \class vpTrackingException + * \ingroup group_core_debug + * \brief Error that can be emitted by the vpTracker class and its derivatives. */ class VISP_EXPORT vpTrackingException : public vpException { public: /*! - \brief Lists the possible error than can be emitted while calling - vpTracking member - */ - enum errorTrackingCodeEnum { - featureLostError, - + * \brief Lists the possible error than can be emitted while calling + * vpTracking member + */ + enum errorTrackingCodeEnum + { + featureLostError, //!< Tracker lost feature // Moving edges - notEnoughPointError, - initializationError, - fatalError + notEnoughPointError, //!< Not enough point to track + initializationError, //!< Tracker initialization error + fatalError //!< Tracker fatal error }; public: + /*! + * Constructor. + */ vpTrackingException(int id, const char *format, ...) { this->code = id; @@ -88,7 +76,15 @@ class VISP_EXPORT vpTrackingException : public vpException setMessage(format, args); va_end(args); } + + /*! + * Constructor. + */ vpTrackingException(int id, const std::string &msg) : vpException(id, msg) { } + + /*! + * Constructor. + */ explicit vpTrackingException(int id) : vpException(id) { } }; diff --git a/modules/core/include/visp3/core/vpTranslationVector.h b/modules/core/include/visp3/core/vpTranslationVector.h index 0af1b6d39a..a48d24b509 100644 --- a/modules/core/include/visp3/core/vpTranslationVector.h +++ b/modules/core/include/visp3/core/vpTranslationVector.h @@ -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: * Translation vector. - * -*****************************************************************************/ + */ #ifndef vpTRANSLATIONVECTOR_H #define vpTRANSLATIONVECTOR_H diff --git a/modules/core/include/visp3/core/vpTriangle.h b/modules/core/include/visp3/core/vpTriangle.h index 3424b91a9a..fd7ec5b651 100644 --- a/modules/core/include/visp3/core/vpTriangle.h +++ b/modules/core/include/visp3/core/vpTriangle.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,10 +29,7 @@ * * Description: * Defines a 2D triangle. - * - - * -*****************************************************************************/ + */ #ifndef vpTriangle_h #define vpTriangle_h diff --git a/modules/core/include/visp3/core/vpUDPClient.h b/modules/core/include/visp3/core/vpUDPClient.h index dcd1365834..f48b31a7ef 100644 --- a/modules/core/include/visp3/core/vpUDPClient.h +++ b/modules/core/include/visp3/core/vpUDPClient.h @@ -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: * UDP Client - * -*****************************************************************************/ + */ #ifndef _vpUDPClient_h_ #define _vpUDPClient_h_ @@ -127,11 +125,11 @@ int main() { #include #include -struct DataType { +struct vpDataType_t { double double_val; int int_val; - DataType() : double_val(0.0), int_val(0) {} - DataType(double dbl, int i) : double_val(dbl), int_val(i) {} + vpDataType_t() : double_val(0.0), int_val(0) {} + vpDataType_t(double dbl, int i) : double_val(dbl), int_val(i) {} }; int main() { @@ -139,7 +137,7 @@ int main() { std::string servername = "127.0.0.1"; unsigned int port = 50037; vpUDPClient client(servername, port); - DataType data_type(1234.56789, 123450); + vpDataType_t data_type(1234.56789, 123450); char data[sizeof(data_type.double_val)+sizeof(data_type.int_val)]; memcpy(data, &data_type.double_val, sizeof(data_type.double_val)); diff --git a/modules/core/include/visp3/core/vpUDPServer.h b/modules/core/include/visp3/core/vpUDPServer.h index 67920c70c1..351f20a2f7 100644 --- a/modules/core/include/visp3/core/vpUDPServer.h +++ b/modules/core/include/visp3/core/vpUDPServer.h @@ -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: * UDP Server - * -*****************************************************************************/ + */ #ifndef _vpUDPServer_h_ #define _vpUDPServer_h_ @@ -144,12 +142,12 @@ receive): #include #include -struct DataType { +struct vpDataType_t { double double_val; int int_val; - DataType() : double_val(0.0), int_val(0) {} - DataType(double dbl, int i) : double_val(dbl), int_val(i) {} + vpDataType_t() : double_val(0.0), int_val(0) {} + vpDataType_t(double dbl, int i) : double_val(dbl), int_val(i) {} }; int main() { @@ -160,7 +158,7 @@ int main() { std::string msg = "", hostInfo = ""; int res = server.receive(msg, hostInfo); if (res) { - DataType data_type; + vpDataType_t data_type; memcpy(&data_type.double_val, msg.c_str(), sizeof(data_type.double_val)); memcpy(&data_type.int_val, msg.c_str()+sizeof(data_type.double_val), sizeof(data_type.int_val)); diff --git a/modules/core/include/visp3/core/vpUniRand.h b/modules/core/include/visp3/core/vpUniRand.h index 29574ec287..8b17b16a24 100644 --- a/modules/core/include/visp3/core/vpUniRand.h +++ b/modules/core/include/visp3/core/vpUniRand.h @@ -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: * Pseudo random number generator. - * -*****************************************************************************/ + */ /* * PCG Random Number Generation for C. * @@ -75,11 +73,11 @@ typedef unsigned __int32 uint32_t; #endif #if (VISP_CXX_STANDARD <= VISP_CXX_STANDARD_11) - #include // std::random_shuffle +#include // std::random_shuffle #else - #include // std::shuffle - #include // std::mt19937 - #include // std::iota +#include // std::shuffle +#include // std::mt19937 +#include // std::iota #endif #include @@ -116,24 +114,24 @@ typedef unsigned __int32 uint32_t; 4.86741 2 5.65826 - Original vector = [ 0 1 2 3 4 5 6 7 8 9 ] - Shuffled vector = [ 2 4 7 8 5 1 3 6 9 0 ] + Original vector = [ 0 1 2 3 4 5 6 7 8 9 ] + Shuffled vector = [ 2 4 7 8 5 1 3 6 9 0 ] \endcode */ class VISP_EXPORT vpUniRand { private: - struct pcg_state_setseq_64 { // Internals are *Private*. + struct vpPcgStateSetseq_64_t + { // 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. - pcg_state_setseq_64(uint64_t state_ = 0x853c49e6748fea9bULL, uint64_t inc_ = 0xda3e39cb94b95bdbULL) + vpPcgStateSetseq_64_t(uint64_t state_ = 0x853c49e6748fea9bULL, uint64_t inc_ = 0xda3e39cb94b95bdbULL) : state(state_), inc(inc_) - { - } + { } }; - typedef struct pcg_state_setseq_64 pcg32_random_t; + typedef struct vpPcgStateSetseq_64_t pcg32_random_t; public: vpUniRand(); @@ -158,11 +156,11 @@ class VISP_EXPORT vpUniRand inline static std::vector shuffleVector(const std::vector &inputVector) { std::vector shuffled = inputVector; - #if (VISP_CXX_STANDARD <= VISP_CXX_STANDARD_11) - std::random_shuffle ( shuffled.begin(), shuffled.end() ); - #else - std::shuffle(shuffled.begin(), shuffled.end(), std::mt19937{std::random_device{}()}); - #endif +#if (VISP_CXX_STANDARD <= VISP_CXX_STANDARD_11) + std::random_shuffle(shuffled.begin(), shuffled.end()); +#else + std::shuffle(shuffled.begin(), shuffled.end(), std::mt19937 { std::random_device{}() }); +#endif return shuffled; } diff --git a/modules/core/include/visp3/core/vpVelocityTwistMatrix.h b/modules/core/include/visp3/core/vpVelocityTwistMatrix.h index 756a492559..55530abdd6 100644 --- a/modules/core/include/visp3/core/vpVelocityTwistMatrix.h +++ b/modules/core/include/visp3/core/vpVelocityTwistMatrix.h @@ -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: * Velocity twist transformation matrix. - * -*****************************************************************************/ + */ #ifndef vpVelocityTwistMatrix_h #define vpVelocityTwistMatrix_h diff --git a/modules/core/include/visp3/core/vpXmlParser.h b/modules/core/include/visp3/core/vpXmlParser.h index 7dac992018..e5970f1177 100644 --- a/modules/core/include/visp3/core/vpXmlParser.h +++ b/modules/core/include/visp3/core/vpXmlParser.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -29,12 +28,8 @@ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. * * Description: - * Tools to automatise the creation of xml parser based on the libXML2 - * - * Authors: - * Romain Tallonneau - * -*****************************************************************************/ + * Tools to automatize the creation of xml parser based on the libXML2 + */ #ifndef vpXmlParser_HH #define vpXmlParser_HH diff --git a/modules/core/include/visp3/core/vpXmlParserCamera.h b/modules/core/include/visp3/core/vpXmlParserCamera.h index 0715f1b2c9..838a5f378d 100644 --- a/modules/core/include/visp3/core/vpXmlParserCamera.h +++ b/modules/core/include/visp3/core/vpXmlParserCamera.h @@ -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: * XML parser to load and save camera intrinsic parameters. - * -*****************************************************************************/ + */ /*! \file vpXmlParserCamera.h diff --git a/modules/core/include/visp3/core/vpXmlParserHomogeneousMatrix.h b/modules/core/include/visp3/core/vpXmlParserHomogeneousMatrix.h index f23792e322..567c8fb631 100644 --- a/modules/core/include/visp3/core/vpXmlParserHomogeneousMatrix.h +++ b/modules/core/include/visp3/core/vpXmlParserHomogeneousMatrix.h @@ -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: * XML parser to load and save Homogeneous Matrix in a XML file. - * -*****************************************************************************/ + */ /*! \file vpXmlParserHomogeneousMatrix.h diff --git a/modules/core/include/visp3/core/vpXmlParserRectOriented.h b/modules/core/include/visp3/core/vpXmlParserRectOriented.h index d290db48be..ce1073efe4 100644 --- a/modules/core/include/visp3/core/vpXmlParserRectOriented.h +++ b/modules/core/include/visp3/core/vpXmlParserRectOriented.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -29,12 +28,8 @@ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. * * Description: - * XML parser to load and save oriented rectangles in a XML file - * - * Authors: - * Marc Pouliquen - * -*****************************************************************************/ + * XML parser to load and save oriented rectangles in a XML file. + */ /*! \file vpXmlParserRectOriented.h diff --git a/modules/core/src/camera/vpCameraParameters.cpp b/modules/core/src/camera/vpCameraParameters.cpp index 8c7a6c8ca5..1ef31179eb 100644 --- a/modules/core/src/camera/vpCameraParameters.cpp +++ b/modules/core/src/camera/vpCameraParameters.cpp @@ -187,8 +187,8 @@ int main() It produces the following output: \code Camera parameters for perspective projection without distortion: - px = 600 py = 600 - u0 = 320 v0 = 240 + px = 600 py = 600 + u0 = 320 v0 = 240 Field of view (horizontal: 56.145 and vertical: 43.6028 degrees) \endcode @@ -251,8 +251,8 @@ int main() It produces the following output: \code Camera parameters for perspective projection with distortion: - px = 600 py = 600 - u0 = 320 v0 = 240 + px = 600 py = 600 + u0 = 320 v0 = 240 kud = -0.19 kdu = 0.2 @@ -377,8 +377,8 @@ int main() It produces the following output: \code Camera parameters for perspective projection without distortion: - px = 601.832 py = 609.275 - u0 = 320 v0 = 240 + px = 601.832 py = 609.275 + u0 = 320 v0 = 240 Field of view (horizontal: 56 and vertical: 43 degrees) \endcode diff --git a/modules/core/src/image/vpColormap.cpp b/modules/core/src/image/vpColormap.cpp index 9465601a46..982ac9eb6d 100644 --- a/modules/core/src/image/vpColormap.cpp +++ b/modules/core/src/image/vpColormap.cpp @@ -1,7 +1,6 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2022 Inria. All rights reserved. + * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * * This software is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -30,9 +29,8 @@ * * Description: * Colormap class to recolor an image with different grayscale values into - * some corresponding color values, for better visualisation for example. - * -*****************************************************************************/ + * some corresponding color values, for better visualization for example. + */ #include diff --git a/modules/core/src/image/vpImageConvert.cpp b/modules/core/src/image/vpImageConvert.cpp index 90f76332c2..f1d6df0bf0 100644 --- a/modules/core/src/image/vpImageConvert.cpp +++ b/modules/core/src/image/vpImageConvert.cpp @@ -3946,16 +3946,16 @@ void vpImageConvert::split(const vpImage &src, vpImage *p src.getWidth()); if (!pR) { - delete [] ptrR; + delete[] ptrR; } if (!pG) { - delete [] ptrG; + delete[] ptrG; } if (!pB) { - delete [] ptrB; + delete[] ptrB; } if (!pa) { - delete [] ptrA; + delete[] ptrA; } } } diff --git a/modules/core/src/math/kalman/vpLinearKalmanFilterInstantiation.cpp b/modules/core/src/math/kalman/vpLinearKalmanFilterInstantiation.cpp index fe71a5d43f..9186d0a69c 100644 --- a/modules/core/src/math/kalman/vpLinearKalmanFilterInstantiation.cpp +++ b/modules/core/src/math/kalman/vpLinearKalmanFilterInstantiation.cpp @@ -814,7 +814,6 @@ void vpLinearKalmanFilterInstantiation::filter(vpColVector &z) if (model == stateConstVel_MeasurePos) { for (unsigned int i = 0; i < size_measure * nsignal; i++) { double z_prev = Xest[size_state * i]; // Previous mesured position - // std::cout << "Mesure pre: " << z_prev << std::endl; Xest[size_state * i] = z[i]; Xest[size_state * i + 1] = (z[i] - z_prev) / dt; } diff --git a/modules/core/src/math/matrix/vpMatrix.cpp b/modules/core/src/math/matrix/vpMatrix.cpp index 25979fb4ac..99d1b7b83b 100644 --- a/modules/core/src/math/matrix/vpMatrix.cpp +++ b/modules/core/src/math/matrix/vpMatrix.cpp @@ -1543,7 +1543,6 @@ void vpMatrix::negateMatrix(const vpMatrix &A, vpMatrix &C) double **ArowPtrs = A.rowPtrs; double **CrowPtrs = C.rowPtrs; - // t0=vpTime::measureTimeMicros(); for (unsigned int i = 0; i < A.rowNum; i++) for (unsigned int j = 0; j < A.colNum; j++) CrowPtrs[i][j] = -ArowPtrs[i][j]; diff --git a/modules/core/src/math/matrix/vpMatrix_covariance.cpp b/modules/core/src/math/matrix/vpMatrix_covariance.cpp index 762b04937c..24b0565f04 100644 --- a/modules/core/src/math/matrix/vpMatrix_covariance.cpp +++ b/modules/core/src/math/matrix/vpMatrix_covariance.cpp @@ -48,7 +48,7 @@ /*! Compute the covariance matrix of the parameters x from a least squares - minimisation defined as: Ax = b + minimization defined as: Ax = b \param A : Matrix A from Ax = b. @@ -77,7 +77,7 @@ vpMatrix vpMatrix::computeCovarianceMatrix(const vpMatrix &A, const vpColVector /*! Compute the covariance matrix of the parameters x from a least squares - minimisation defined as: WAx = Wb + minimization defined as: WAx = Wb \param A : Matrix A from WAx = Wb. diff --git a/modules/core/src/math/misc/vpMath.cpp b/modules/core/src/math/misc/vpMath.cpp index 1801681781..ba6b5a12a0 100644 --- a/modules/core/src/math/misc/vpMath.cpp +++ b/modules/core/src/math/misc/vpMath.cpp @@ -478,9 +478,18 @@ vpHomogeneousMatrix vpMath::ned2ecef(double lonDeg, double latDeg, double radius double lat = vpMath::rad(latDeg); 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[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); return ecef_M_ned; } @@ -530,9 +539,18 @@ vpHomogeneousMatrix vpMath::enu2ecef(double lonDeg, double latDeg, double radius double lat = vpMath::rad(latDeg); 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[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); return ecef_M_enu; } @@ -646,9 +664,18 @@ vpHomogeneousMatrix vpMath::lookAt(const vpColVector &from, const vpColVector &t vpColVector up = vpColVector::crossProd(forward, right).normalize(); 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[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]; return wMc; } diff --git a/modules/core/src/math/spline/vpBSpline.cpp b/modules/core/src/math/spline/vpBSpline.cpp index 7370bddd8d..408757f339 100644 --- a/modules/core/src/math/spline/vpBSpline.cpp +++ b/modules/core/src/math/spline/vpBSpline.cpp @@ -44,9 +44,8 @@ */ vpBSpline::vpBSpline() : controlPoints(), knots(), p(3), // By default : p=3 for clubic spline - crossingPoints() -{ -} + crossingPoints() +{ } /*! Copy constructor. @@ -54,13 +53,12 @@ vpBSpline::vpBSpline() */ vpBSpline::vpBSpline(const vpBSpline &bspline) : controlPoints(bspline.controlPoints), knots(bspline.knots), p(bspline.p), // By default : p=3 for clubic spline - crossingPoints(bspline.crossingPoints) -{ -} + crossingPoints(bspline.crossingPoints) +{ } /*! Basic destructor. */ -vpBSpline::~vpBSpline() {} +vpBSpline::~vpBSpline() { } /*! Find the knot interval in which the parameter \f$ l_u \f$ lies. Indeed \f$ @@ -68,9 +66,9 @@ vpBSpline::~vpBSpline() {} Example : The knot vector is the following \f$ U = \{0, 0 , 1 , 2 ,3 , 3\} \f$ with \f$ p \f$ is equal to 1. - - For \f$ l_u \f$ equal to 0.5 the method will retun 1. - - For \f$ l_u \f$ equal to 2.5 the method will retun 3. - - For \f$ l_u \f$ equal to 3 the method will retun 3. + - For \f$ l_u \f$ equal to 0.5 the method will return 1. + - For \f$ l_u \f$ equal to 2.5 the method will return 3. + - For \f$ l_u \f$ equal to 3 the method will return 3. \param l_u : The knot whose knot interval is seeked. \param l_p : Degree of the B-Spline basis functions. @@ -114,9 +112,9 @@ unsigned int vpBSpline::findSpan(double l_u, unsigned int l_p, std::vector #include #include "cpu_x86.h" -namespace FeatureDetector{ +namespace FeatureDetector +{ //////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////// @@ -39,7 +40,7 @@ uint64_t _xgetbv(unsigned int ext_ctrl_reg) mov ecx, [ext_ctrl_reg] __asm _emit 0x0f __asm _emit 0x01 __asm _emit 0xd0 /* xgetbv() */ mov eax_, eax - mov edx_, edx + mov edx_, edx } return ((uint64_t)edx_ << 32) | eax_; } @@ -53,69 +54,70 @@ uint64_t _xgetbv(unsigned int) unsigned __int64 _xgetbv(unsigned int index) { #if defined(__x86_64__) || defined(_AMD64_) - unsigned __int64 val1, val2; + unsigned __int64 val1, val2; #else - unsigned __LONG32 val1, val2; + unsigned __LONG32 val1, val2; #endif /* defined(__x86_64__) || defined(_AMD64_) */ - __asm__ __volatile__( - "xgetbv" - : "=a" (val1), "=d" (val2) - : "c" (index)); + __asm__ __volatile__( + "xgetbv" + : "=a" (val1), "=d" (val2) + : "c" (index)); - return (((unsigned __int64)val2) << 32) | val1; + return (((unsigned __int64)val2) << 32) | val1; } #endif #if defined(__MINGW32__) -void __cpuidex(int CPUInfo[4], int function_id, int subfunction_id) { - __asm__ __volatile__ ( - "cpuid" - : "=a" (CPUInfo [0]), "=b" (CPUInfo [1]), "=c" (CPUInfo [2]), "=d" (CPUInfo [3]) - : "a" (function_id), "c" (subfunction_id)); +void __cpuidex(int CPUInfo[4], int function_id, int subfunction_id) +{ + __asm__ __volatile__( + "cpuid" + : "=a" (CPUInfo[0]), "=b" (CPUInfo[1]), "=c" (CPUInfo[2]), "=d" (CPUInfo[3]) + : "a" (function_id), "c" (subfunction_id)); } #endif -void cpu_x86::cpuid(int32_t out[4], int32_t x){ - __cpuidex(out, x, 0); +void cpu_x86::cpuid(int32_t out[4], int32_t x) +{ + __cpuidex(out, x, 0); } -__int64 xgetbv(unsigned int x){ - return _xgetbv(x); +__int64 xgetbv(unsigned int x) +{ + return _xgetbv(x); } //////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////// // Detect 64-bit - Note that this snippet of code for detecting 64-bit has been copied from MSDN. -typedef BOOL (WINAPI *LPFN_ISWOW64PROCESS) (HANDLE, PBOOL); +typedef BOOL(WINAPI *LPFN_ISWOW64PROCESS) (HANDLE, PBOOL); BOOL IsWow64() { - BOOL bIsWow64 = FALSE; + BOOL bIsWow64 = FALSE; #if defined(__MINGW__) || defined(__MINGW32__) || defined(__MINGW64__) - if (!IsWow64Process(GetCurrentProcess(), &bIsWow64)) - { - printf("Error Detecting Operating System.\n"); - printf("Defaulting to 32-bit OS.\n\n"); - bIsWow64 = FALSE; - } + if (!IsWow64Process(GetCurrentProcess(), &bIsWow64)) { + printf("Error Detecting Operating System.\n"); + printf("Defaulting to 32-bit OS.\n\n"); + bIsWow64 = FALSE; + } #elif !defined(WINRT) // Turned off on UWP where GetModuleHandle() doesn't exist - LPFN_ISWOW64PROCESS fnIsWow64Process = (LPFN_ISWOW64PROCESS) GetProcAddress( - GetModuleHandle(TEXT("kernel32")), "IsWow64Process"); + LPFN_ISWOW64PROCESS fnIsWow64Process = (LPFN_ISWOW64PROCESS)GetProcAddress( + GetModuleHandle(TEXT("kernel32")), "IsWow64Process"); - if (NULL != fnIsWow64Process) - { - if (!fnIsWow64Process(GetCurrentProcess(), &bIsWow64)) - { - printf("Error Detecting Operating System.\n"); - printf("Defaulting to 32-bit OS.\n\n"); - bIsWow64 = FALSE; - } + if (NULL != fnIsWow64Process) { + if (!fnIsWow64Process(GetCurrentProcess(), &bIsWow64)) { + printf("Error Detecting Operating System.\n"); + printf("Defaulting to 32-bit OS.\n\n"); + bIsWow64 = FALSE; } + } #endif - return bIsWow64; + return bIsWow64; } -bool cpu_x86::detect_OS_x64(){ +bool cpu_x86::detect_OS_x64() +{ #ifdef _M_X64 - return true; + return true; #else - return IsWow64() != 0; + return IsWow64() != 0; #endif } //////////////////////////////////////////////////////////////////////////////// diff --git a/modules/core/src/tools/exceptions/vpException.cpp b/modules/core/src/tools/exceptions/vpException.cpp index 7c5804e92d..8dd97bf362 100644 --- a/modules/core/src/tools/exceptions/vpException.cpp +++ b/modules/core/src/tools/exceptions/vpException.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,25 +29,16 @@ * * Description: * Exception handling. - * - * Authors: - * Nicolas Mansard - * -*****************************************************************************/ + */ -/* \file vpException.cpp - \brief error that can be emitted by the vp class and its derivatives +/*! + * \file vpException.cpp + * \brief error that can be emitted by the vp class and its derivatives */ #include "visp3/core/vpException.h" #include -/* ------------------------------------------------------------------------- - */ -/* --- CONSTRUCTORS -------------------------------------------------------- - */ -/* ------------------------------------------------------------------------- - */ vpException::vpException(int id) : code(id), message() {} vpException::vpException(int id, const std::string &msg) : code(id), message(msg) {} @@ -62,15 +52,6 @@ vpException::vpException(int id, const char *format, ...) : code(id), message() } vpException::vpException(int id, const char *format, va_list args) : code(id), message() { setMessage(format, args); } -/* ------------------------------------------------------------------------ */ -/* --- DESTRUCTORS -------------------------------------------------------- */ -/* ------------------------------------------------------------------------ */ - -/* Destructeur par default suffisant. */ -// vpException:: -// ~vpException (void) -// { -// } void vpException::setMessage(const char *format, va_list args) { @@ -80,47 +61,17 @@ void vpException::setMessage(const char *format, va_list args) message = msg; } -/* ------------------------------------------------------------------------ */ -/* --- ACCESSORS ---------------------------------------------------------- */ -/* ------------------------------------------------------------------------ */ - const char *vpException::getMessage() const { return (this->message).c_str(); } const std::string &vpException::getStringMessage() const { return this->message; } int vpException::getCode() const { return this->code; } -/*! - Overloading of the what() method of std::exception to return the vpException - message. - - \return pointer on the array of \e char related to the error string. -*/ const char *vpException::what() const throw() { return (this->message).c_str(); } -/* ------------------------------------------------------------------------- - */ -/* --- MODIFIORS ----------------------------------------------------------- - */ -/* ------------------------------------------------------------------------- - */ - -/* ------------------------------------------------------------------------- - */ -/* --- OP << --------------------------------------------------------------- - */ -/* ------------------------------------------------------------------------- - */ - VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpException &error) { os << "Error [" << error.code << "]:\t" << error.message << std::endl; return os; } - -/* - * Local variables: - * c-basic-offset: 2 - * End: - */ diff --git a/modules/core/src/tools/file/vpIoTools.cpp b/modules/core/src/tools/file/vpIoTools.cpp index d2d4dd70bf..11910f1817 100644 --- a/modules/core/src/tools/file/vpIoTools.cpp +++ b/modules/core/src/tools/file/vpIoTools.cpp @@ -131,7 +131,9 @@ void replaceAll(std::string &str, const std::string &search, const std::string & std::string <rim(std::string &s) { #if VISP_CXX_STANDARD > VISP_CXX_STANDARD_98 - s.erase(s.begin(), std::find_if(s.begin(), s.end(), [](int c) { return !std::isspace(c); })); + s.erase(s.begin(), std::find_if(s.begin(), s.end(), [](int c) { + return !std::isspace(c); + })); #else s.erase(s.begin(), std::find_if(s.begin(), s.end(), std::not1(std::ptr_fun(std::isspace)))); #endif @@ -141,7 +143,9 @@ std::string <rim(std::string &s) std::string &rtrim(std::string &s) { #if VISP_CXX_STANDARD > VISP_CXX_STANDARD_98 - s.erase(std::find_if(s.rbegin(), s.rend(), [](int c) { return !std::isspace(c); }).base(), s.end()); + s.erase(std::find_if(s.rbegin(), s.rend(), [](int c) { + return !std::isspace(c); + }).base(), s.end()); #else s.erase(std::find_if(s.rbegin(), s.rend(), std::not1(std::ptr_fun(std::isspace))).base(), s.end()); #endif diff --git a/modules/core/src/tools/histogram/vpHistogram.cpp b/modules/core/src/tools/histogram/vpHistogram.cpp index 1513fa40f7..9d34be9dd7 100644 --- a/modules/core/src/tools/histogram/vpHistogram.cpp +++ b/modules/core/src/tools/histogram/vpHistogram.cpp @@ -52,7 +52,8 @@ namespace { -struct Histogram_Param_t { +struct vpHistogram_Param_t +{ unsigned int m_start_index; unsigned int m_end_index; @@ -60,14 +61,13 @@ struct Histogram_Param_t { unsigned int *m_histogram; const vpImage *m_I; - Histogram_Param_t() : m_start_index(0), m_end_index(0), m_lut(), m_histogram(NULL), m_I(NULL) {} + vpHistogram_Param_t() : m_start_index(0), m_end_index(0), m_lut(), m_histogram(NULL), m_I(NULL) { } - Histogram_Param_t(unsigned int start_index, unsigned int end_index, const vpImage *const I) + vpHistogram_Param_t(unsigned int start_index, unsigned int end_index, const vpImage *const I) : m_start_index(start_index), m_end_index(end_index), m_lut(), m_histogram(NULL), m_I(I) - { - } + { } - ~Histogram_Param_t() + ~vpHistogram_Param_t() { if (m_histogram != NULL) { delete[] m_histogram; @@ -77,7 +77,7 @@ struct Histogram_Param_t { vpThread::Return computeHistogramThread(vpThread::Args args) { - Histogram_Param_t *histogram_param = static_cast(args); + vpHistogram_Param_t *histogram_param = static_cast(args); unsigned int start_index = histogram_param->m_start_index; unsigned int end_index = histogram_param->m_end_index; @@ -271,12 +271,13 @@ void vpHistogram::calculate(const vpImage &I, unsigned int nbins, histogram[lut[*ptrCurrent]]++; ++ptrCurrent; } - } else { + } + else { #if defined(VISP_HAVE_PTHREAD) || (defined(_WIN32) && !defined(WINRT_8_0)) // Multi-threads std::vector threadpool; - std::vector histogramParams; + std::vector histogramParams; unsigned int image_size = I.getSize(); unsigned int step = image_size / nbThreads; @@ -290,7 +291,7 @@ void vpHistogram::calculate(const vpImage &I, unsigned int nbins, end_index = start_index + last_step; } - Histogram_Param_t *histogram_param = new Histogram_Param_t(start_index, end_index, &I); + vpHistogram_Param_t *histogram_param = new vpHistogram_Param_t(start_index, end_index, &I); histogram_param->m_histogram = new unsigned int[size]; memset(histogram_param->m_histogram, 0, size * sizeof(unsigned int)); memcpy(histogram_param->m_lut, lut, 256 * sizeof(unsigned int)); @@ -601,11 +602,6 @@ bool vpHistogram::getPeaks(unsigned char dist, vpHistogramPeak &peakl, vpHistogr if (prev_slope > 0) peak[nbpeaks++] = (unsigned char)(size - 1); - // vpTRACE("nb peaks: %d", nbpeaks); - // for (unsigned i=0; i < nbpeaks; i ++) - // vpTRACE("peak %d: pos %d value: %d", i, peak[i], histogram[ peak[i] - // ]); - // Get the global maximum index_highest_peak = 0; for (unsigned int i = 0; i < nbpeaks; i++) { @@ -614,10 +610,6 @@ bool vpHistogram::getPeaks(unsigned char dist, vpHistogramPeak &peakl, vpHistogr } } - // vpTRACE("highest peak index: %d pos: %d value: %d", - // index_highest_peak, peak[index_highest_peak], - // histogram[ peak[index_highest_peak] ]); - maxprof = 0; index_second_peak = index_highest_peak; @@ -650,15 +642,13 @@ bool vpHistogram::getPeaks(unsigned char dist, vpHistogramPeak &peakl, vpHistogr } } } - // vpTRACE("second peak index: %d pos: %d value: %d", - // index_second_peak, peak[index_second_peak], - // histogram[ peak[index_second_peak] ]); // Determine position of the first and second highest peaks if (peak[index_highest_peak] < peak[index_second_peak]) { peakr.set(peak[index_second_peak], histogram[peak[index_second_peak]]); peakl.set(peak[index_highest_peak], histogram[peak[index_highest_peak]]); - } else { + } + else { peakl.set(peak[index_second_peak], histogram[peak[index_second_peak]]); peakr.set(peak[index_highest_peak], histogram[peak[index_highest_peak]]); } @@ -697,7 +687,8 @@ bool vpHistogram::getPeaks(unsigned char dist, vpHistogramPeak &peakl, vpHistogr delete[] peak; return false; - } else { + } + else { mini = sumindmini / nbmini; // mean valey.set((unsigned char)mini, histogram[mini]); @@ -795,7 +786,8 @@ bool vpHistogram::getValey(const vpHistogramPeak &peak1, const vpHistogramPeak & if (peak1.getLevel() > peak2.getLevel()) { peakl = peak2; peakr = peak1; - } else { + } + else { peakl = peak1; peakr = peak2; } @@ -826,7 +818,8 @@ bool vpHistogram::getValey(const vpHistogramPeak &peak1, const vpHistogramPeak & valey.set(0, 0); return false; - } else { + } + else { unsigned int minipos = sumindmini / nbmini; // position of the minimum valey.set((unsigned char)minipos, histogram[minipos]); @@ -879,16 +872,6 @@ unsigned vpHistogram::getValey(unsigned char dist, const vpHistogramPeak &peak, /* nbpeaks = */ getPeaks(peaks); } - // if (1) { - // // vpTRACE("nb peaks: %d", nbpeaks); - // peaks.front(); - // for (unsigned i=0; i < nbpeaks; i ++) { - // vpHistogramPeak p = peaks.value(); - // // vpTRACE("peak index %d: pos %d value: %d", - // // i, p.getLevel(), p.getValue()); - // peaks.next(); - // } - // } // Go to the requested peak in the list std::list::const_iterator it; unsigned index = 0; @@ -905,8 +888,9 @@ unsigned vpHistogram::getValey(unsigned char dist, const vpHistogramPeak &peak, // No chance to get a peak on the left // should not occur ! peakl.set(0, 0); - } else { - // search for the nearest peak on the left that matches the distance + } + else { + // search for the nearest peak on the left that matches the distance std::list::const_iterator lit; // left iterator for (lit = peaks.begin(); lit != it; ++lit) { if (abs((*lit).getLevel() - peak.getLevel()) > dist) { @@ -939,7 +923,8 @@ unsigned vpHistogram::getValey(unsigned char dist, const vpHistogramPeak &peak, if (nbmini == 0) { valeyl.set(0, 0); ret &= 0x01; - } else { + } + else { unsigned int minipos = sumindmini / nbmini; // position of the minimum valeyl.set((unsigned char)minipos, histogram[minipos]); ret &= 0x11; @@ -994,7 +979,8 @@ unsigned vpHistogram::getValey(unsigned char dist, const vpHistogramPeak &peak, if (nbmini == 0) { valeyr.set((unsigned char)(size - 1), 0); ret &= 0x10; - } else { + } + else { unsigned int minipos = sumindmini / nbmini; // position of the minimum valeyr.set((unsigned char)minipos, histogram[minipos]); ret &= 0x11; diff --git a/modules/core/src/tools/network/vpNetwork.cpp b/modules/core/src/tools/network/vpNetwork.cpp index 663181f382..abfaf86dee 100644 --- a/modules/core/src/tools/network/vpNetwork.cpp +++ b/modules/core/src/tools/network/vpNetwork.cpp @@ -43,8 +43,8 @@ vpNetwork::vpNetwork() : emitter(), receptor_list(), readFileDescriptor(), socketMax(0), request_list(), max_size_message(999999), - separator("[*@*]"), beginning("[*start*]"), end("[*end*]"), param_sep("[*|*]"), currentMessageReceived(), tv(), - tv_sec(0), tv_usec(10), verboseMode(false) + separator("[*@*]"), beginning("[*start*]"), end("[*end*]"), param_sep("[*|*]"), currentMessageReceived(), tv(), + tv_sec(0), tv_usec(10), verboseMode(false) { tv.tv_sec = tv_sec; #ifdef TARGET_OS_IPHONE @@ -92,8 +92,8 @@ void vpNetwork::addDecodingRequest(vpRequest *req) if (alreadyHas) std::cout << "Server already has one request with the similar ID. " - "Request hasn't been added." - << std::endl; + "Request hasn't been added." + << std::endl; else request_list.push_back(req); } @@ -265,7 +265,7 @@ int vpNetwork::sendAndEncodeRequestTo(vpRequest &req, const unsigned int &dest) } /*! - Receive requests untils there is requests to receive. + Receive requests until there is requests to receive. \warning Requests will be received but not decoded. @@ -280,12 +280,12 @@ int vpNetwork::sendAndEncodeRequestTo(vpRequest &req, const unsigned int &dest) */ std::vector vpNetwork::receiveRequest() { - _receiveRequest(); - return _handleRequests(); + privReceiveRequest(); + return privHandleRequests(); } /*! - Receives requests, from a specific emitter, untils there is request to + Receives requests, from a specific emitter, until there is request to receive. \warning Requests will be received but not decoded. @@ -303,8 +303,8 @@ std::vector vpNetwork::receiveRequest() */ std::vector vpNetwork::receiveRequestFrom(const unsigned int &receptorEmitting) { - _receiveRequestFrom(receptorEmitting); - return _handleRequests(); + privReceiveRequestFrom(receptorEmitting); + return privHandleRequests(); } /*! @@ -327,8 +327,8 @@ std::vector vpNetwork::receiveRequestFrom(const unsigned int &receptorEmitt */ int vpNetwork::receiveRequestOnce() { - _receiveRequestOnce(); - return _handleFirstRequest(); + privReceiveRequestOnce(); + return privHandleFirstRequest(); } /*! @@ -354,12 +354,12 @@ int vpNetwork::receiveRequestOnce() */ int vpNetwork::receiveRequestOnceFrom(const unsigned int &receptorEmitting) { - _receiveRequestOnceFrom(receptorEmitting); - return _handleFirstRequest(); + privReceiveRequestOnceFrom(receptorEmitting); + return privHandleFirstRequest(); } /*! - Receives and decode requests untils there is requests to receive. + Receives and decode requests until there is requests to receive. \warning Requests will be received but not decoded. @@ -383,7 +383,7 @@ std::vector vpNetwork::receiveAndDecodeRequest() } /*! - Receives and decode requests, from a specific emitter, untils there is + Receives and decode requests, from a specific emitter, until there is request to receive. \warning Requests will be received but not decoded. @@ -483,14 +483,14 @@ int vpNetwork::receiveAndDecodeRequestOnceFrom(const unsigned int &receptorEmitt \return : The list of index corresponding to the requests that have been handled. */ -std::vector vpNetwork::_handleRequests() +std::vector vpNetwork::privHandleRequests() { std::vector resIndex; - int index = _handleFirstRequest(); + int index = privHandleFirstRequest(); while (index != -1) { resIndex.push_back(index); - index = _handleFirstRequest(); + index = privHandleFirstRequest(); } return resIndex; @@ -506,7 +506,7 @@ std::vector vpNetwork::_handleRequests() \return : The index of the request that has been handled. */ -int vpNetwork::_handleFirstRequest() +int vpNetwork::privHandleFirstRequest() { size_t indStart = currentMessageReceived.find(beginning); size_t indSep = currentMessageReceived.find(separator); @@ -587,7 +587,7 @@ int vpNetwork::_handleFirstRequest() } /*! - Receive requests untils there is requests to receive. + Receive requests until there is requests to receive. \warning Requests will be received but not decoded. @@ -600,14 +600,14 @@ int vpNetwork::_handleFirstRequest() \sa vpNetwork::receiveAndDecodeRequestOnce() \sa vpNetwork::receiveAndDecodeRequestOnceFrom() */ -void vpNetwork::_receiveRequest() +void vpNetwork::privReceiveRequest() { - while (_receiveRequestOnce() > 0) { + while (privReceiveRequestOnce() > 0) { }; } /*! - Receives requests, from a specific emitter, untils there is request to + Receives requests, from a specific emitter, until there is request to receive. \warning Requests will be received but not decoded. @@ -623,9 +623,9 @@ void vpNetwork::_receiveRequest() \param receptorEmitting : Index of the receptor emitting the message */ -void vpNetwork::_receiveRequestFrom(const unsigned int &receptorEmitting) +void vpNetwork::privReceiveRequestFrom(const unsigned int &receptorEmitting) { - while (_receiveRequestOnceFrom(receptorEmitting) > 0) { + while (privReceiveRequestOnceFrom(receptorEmitting) > 0) { }; } @@ -647,7 +647,7 @@ void vpNetwork::_receiveRequestFrom(const unsigned int &receptorEmitting) \return The number of bytes received, -1 if an error occured. */ -int vpNetwork::_receiveRequestOnce() +int vpNetwork::privReceiveRequestOnce() { if (receptor_list.size() == 0) { if (verboseMode) @@ -680,10 +680,12 @@ int vpNetwork::_receiveRequestOnce() if (verboseMode) vpERROR_TRACE("Select error"); return -1; - } else if (value == 0) { - // Timeout + } + else if (value == 0) { + // Timeout return 0; - } else { + } + else { for (unsigned int i = 0; i < receptor_list.size(); i++) { if (FD_ISSET((unsigned int)receptor_list[i].socketFileDescriptorReceptor, &readFileDescriptor)) { char *buf = new char[max_size_message]; @@ -698,7 +700,8 @@ int vpNetwork::_receiveRequestOnce() receptor_list.erase(receptor_list.begin() + (int)i); delete[] buf; return numbytes; - } else { + } + else { std::string returnVal(buf, (unsigned int)numbytes); currentMessageReceived.append(returnVal); } @@ -732,7 +735,7 @@ int vpNetwork::_receiveRequestOnce() \return The number of bytes received, -1 if an error occured. */ -int vpNetwork::_receiveRequestOnceFrom(const unsigned int &receptorEmitting) +int vpNetwork::privReceiveRequestOnceFrom(const unsigned int &receptorEmitting) { int size = (int)receptor_list.size(); int sizeMinusOne = (int)receptor_list.size() - 1; @@ -760,10 +763,12 @@ int vpNetwork::_receiveRequestOnceFrom(const unsigned int &receptorEmitting) if (verboseMode) vpERROR_TRACE("Select error"); return -1; - } else if (value == 0) { - // Timeout + } + else if (value == 0) { + // Timeout return 0; - } else { + } + else { if (FD_ISSET((unsigned int)receptor_list[receptorEmitting].socketFileDescriptorReceptor, &readFileDescriptor)) { char *buf = new char[max_size_message]; #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX @@ -774,11 +779,12 @@ int vpNetwork::_receiveRequestOnceFrom(const unsigned int &receptorEmitting) #endif if (numbytes <= 0) { std::cout << "Disconnected : " << inet_ntoa(receptor_list[receptorEmitting].receptorAddress.sin_addr) - << std::endl; + << std::endl; receptor_list.erase(receptor_list.begin() + (int)receptorEmitting); delete[] buf; return numbytes; - } else { + } + else { std::string returnVal(buf, (unsigned int)numbytes); currentMessageReceived.append(returnVal); } @@ -791,5 +797,5 @@ int vpNetwork::_receiveRequestOnceFrom(const unsigned int &receptorEmitting) #elif !defined(VISP_BUILD_SHARED_LIBS) // Work around to avoid warning: libvisp_core.a(vpNetwork.cpp.o) has no symbols -void dummy_vpNetwork(){}; +void dummy_vpNetwork() { }; #endif diff --git a/modules/core/src/tools/optimization/vpLinProg.cpp b/modules/core/src/tools/optimization/vpLinProg.cpp index 3345f6394d..7929acd959 100644 --- a/modules/core/src/tools/optimization/vpLinProg.cpp +++ b/modules/core/src/tools/optimization/vpLinProg.cpp @@ -105,7 +105,8 @@ bool vpLinProg::colReduction(vpMatrix &A, vpColVector &b, bool full_rank, const b.resize(n); A.eye(n); return true; - } else + } + else return false; } @@ -127,7 +128,8 @@ bool vpLinProg::colReduction(vpMatrix &A, vpColVector &b, bool full_rank, const return true; } return false; - } else if (r == m) // most common use case - rank is number of rows + } + else if (r == m) // most common use case - rank is number of rows { b = Q * R.inverseTriangular().t() * b; // build projection to kernel of Q^T, pick n-m independent columns of I - Q.Q^T @@ -254,14 +256,15 @@ bool vpLinProg::rowReduction(vpMatrix &A, vpColVector &b, const double &tol) b.resize(0); A.resize(0, n); return true; - } else + } + else return false; } vpMatrix Q, R, P; const unsigned int r = A.qrPivot(Q, R, P, false, false, tol); const vpColVector x = P.transpose() * vpMatrix::stack(R.extract(0, 0, r, r).inverseTriangular(), vpMatrix(n - r, r)) * - Q.extract(0, 0, m, r).transpose() * b; + Q.extract(0, 0, m, r).transpose() * b; if (allClose(A, x, b, tol)) { if (r < m) // if r == m then (A,b) is not changed @@ -345,12 +348,12 @@ bool vpLinProg::solveLP(const vpColVector &c, vpMatrix A, vpColVector b, const v // check if we should forward a feasible point to the next solver const bool feasible = - (x.getRows() == c.getRows()) && (A.getRows() == 0 || allClose(A, x, b, tol)) && - (C.getRows() == 0 || allLesser(C, x, d, tol)) && - (find_if(l.begin(), l.end(), [&](BoundedIndex &i) { return x[i.first] < i.second - tol; }) == l.end()) && - (find_if(u.begin(), u.end(), [&](BoundedIndex &i) { return x[i.first] > i.second + tol; }) == u.end()); + (x.getRows() == c.getRows()) && (A.getRows() == 0 || allClose(A, x, b, tol)) && + (C.getRows() == 0 || allLesser(C, x, d, tol)) && + (find_if(l.begin(), l.end(), [&](BoundedIndex &i) { return x[i.first] < i.second - tol; }) == l.end()) && + (find_if(u.begin(), u.end(), [&](BoundedIndex &i) { return x[i.first] > i.second + tol; }) == u.end()); - // shortcut for unbounded variables with equality +// shortcut for unbounded variables with equality if (!feasible && m && l.size() == 0 && u.size() == 0) { // changes A.x = b to x = b + A.z if (colReduction(A, b, false, tol)) { @@ -359,7 +362,8 @@ bool vpLinProg::solveLP(const vpColVector &c, vpMatrix A, vpColVector b, const v x = b + A * x; return true; } - } else if (C.getRows() && allLesser(C, b, d, tol)) { // A.x = b has only 1 solution (the new b) and C.b <= d + } + else if (C.getRows() && allLesser(C, b, d, tol)) { // A.x = b has only 1 solution (the new b) and C.b <= d x = b; return true; } @@ -371,7 +375,9 @@ bool vpLinProg::solveLP(const vpColVector &c, vpMatrix A, vpColVector b, const v // count how many additional variables are needed to deal with bounds unsigned int s1 = 0, s2 = 0; for (unsigned int i = 0; i < n; ++i) { - const auto cmp = [&](const BoundedIndex &bi) { return bi.first == i; }; + const auto cmp = [&](const BoundedIndex &bi) { + return bi.first == i; + }; // look for lower bound const bool has_low = find_if(l.begin(), l.end(), cmp) != l.end(); // look for upper bound @@ -410,7 +416,9 @@ bool vpLinProg::solveLP(const vpColVector &c, vpMatrix A, vpColVector b, const v unsigned int k1 = 0, k2 = 0; for (unsigned int i = 0; i < n; ++i) { // lambda to find a bound for this index - const auto cmp = [&](const BoundedIndex &bi) { return bi.first == i; }; + const auto cmp = [&](const BoundedIndex &bi) { + return bi.first == i; + }; // look for lower bound const auto low = find_if(l.begin(), l.end(), cmp); @@ -429,7 +437,8 @@ bool vpLinProg::solveLP(const vpColVector &c, vpMatrix A, vpColVector b, const v x[n + p + k1] = std::max(-x[i], 0.); } k1++; - } else // upper bound x <= u <=> z1 = -x + u >= 0 + } + else // upper bound x <= u <=> z1 = -x + u >= 0 { z0[i] = up->second; P[i][i] = -1; @@ -439,7 +448,8 @@ bool vpLinProg::solveLP(const vpColVector &c, vpMatrix A, vpColVector b, const v x[i] = up->second - x[i]; u.erase(up); } - } else // lower bound x >= l <=> z1 = x - l >= 0 + } + else // lower bound x >= l <=> z1 = x - l >= 0 { z0[i] = -low->second; if (up != u.end()) // both bounds z1 + z2 = u - l @@ -453,7 +463,8 @@ bool vpLinProg::solveLP(const vpColVector &c, vpMatrix A, vpColVector b, const v k1++; k2++; u.erase(up); - } else if (feasible) // only lower bound + } + else if (feasible) // only lower bound x[i] = x[i] - low->second; l.erase(low); } @@ -536,11 +547,13 @@ bool vpLinProg::simplex(const vpColVector &c, vpMatrix A, vpColVector b, vpColVe // find a feasible point is passed x is not if ((x.getRows() != c.getRows()) || !allGreater(x, -tol) || (m != 0 && !allClose(A, x, b, tol)) || [&x, &n]() { - unsigned int non0 = 0; // count non-0 in x, feasible if <= m - for (unsigned int i = 0; i < n; ++i) - if (x[i] > 0) - non0++; - return non0; + unsigned int non0 = 0; // count non-0 in x, feasible if <= m + for (unsigned int i = 0; i < n; ++i) { + if (x[i] > 0) { + non0++; + } + } + return non0; }() > m) { // min sum(z) // st A.x + D.z = with diag(D) = sign(b) @@ -554,7 +567,8 @@ bool vpLinProg::simplex(const vpColVector &c, vpMatrix A, vpColVector b, vpColVe if (b[i] > -tol) { AD[i][n + i] = 1; x[n + i] = b[i]; - } else { + } + else { AD[i][n + i] = -1; x[n + i] = -b[i]; } @@ -690,7 +704,7 @@ bool vpLinProg::simplex(const vpColVector &c, vpMatrix A, vpColVector b, vpColVe a.resize(0); for (unsigned int k = 0; k < m; ++k) { if (db[k] < -tol) - a.push_back({-x[B[k]] / db[k], k}); + a.push_back({ -x[B[k]] / db[k], k }); } // get smallest index for smallest alpha const auto amin = std::min_element(a.begin(), a.end()); diff --git a/modules/core/src/tools/serial/vpSerial.cpp b/modules/core/src/tools/serial/vpSerial.cpp index 5cfc70faef..ac6f6273fc 100644 --- a/modules/core/src/tools/serial/vpSerial.cpp +++ b/modules/core/src/tools/serial/vpSerial.cpp @@ -272,7 +272,7 @@ bool vpSerial::read(char *c, long timeout_s) if (ret < 0) { throw(vpException(vpException::fatalError, "Serial i/o exception")); } else if (ret == 0) { - // Timeout occured + // Timeout occurred return false; } else { ssize_t n = ::read(m_fd, c, 1); // read one character at a time @@ -296,7 +296,7 @@ std::string vpSerial::readline(const std::string &eol) while (true) { size_t bytes_read = this->read(&c, 1); if (bytes_read == 0) { - break; // Timeout occured on reading 1 byte + break; // Timeout occurred on reading 1 byte } line.append(&c, 1); if (std::string(line, line.size() - eol_len, eol_len) == eol) { diff --git a/modules/core/src/tools/time/vpTime.cpp b/modules/core/src/tools/time/vpTime.cpp index d5e3faf8d0..408f0c87ab 100644 --- a/modules/core/src/tools/time/vpTime.cpp +++ b/modules/core/src/tools/time/vpTime.cpp @@ -99,7 +99,8 @@ double measureTimeMicros() QueryPerformanceFrequency(&frequency); if (frequency.QuadPart == 0) { return (timeGetTime()); - } else { + } + else { QueryPerformanceCounter(&time); return (double)(1000000.0 * time.QuadPart / frequency.QuadPart); } @@ -132,7 +133,8 @@ double measureTimeMs() QueryPerformanceFrequency(&frequency); if (frequency.QuadPart == 0) { return (timeGetTime()); - } else { + } + else { QueryPerformanceCounter(&time); return (double)(1000.0 * time.QuadPart / frequency.QuadPart); } @@ -280,57 +282,57 @@ void sleepMs(double t) } /*! - Return a string containing date and time. - - \param[in] format : The string format supported by strftime() function that represents the time. - The default format is the following \c "%Y/%m/%d %H:%M:%S". - This string contains any combination of special format specifiers given in the next table: - | specifier | Replaced by | Example | - |-----------|------------------------------------------------------|--------------------------| - | %%a | Abbreviated weekday name * | Thu | - | %%A | Full weekday name * | Thursday | - | %%b | Abbreviated month name * | Aug | - | %%B | Full month name * | August | - | %%c | Date and time representation * | Thu Aug 23 14:55:02 2001 | - | %%C | Year divided by 100 and truncated to integer (00-99) | 20 | - | %%d | Day of the month, zero-padded (01-31) | 23 | - | %%D | Short MM/DD/YY date, equivalent to %m/%d/%y | 08/23/01 | - | %%e | Day of the month, space-padded ( 1-31) | 23 | - | %%F | Short YYYY-MM-DD date, equivalent to %Y-%m-%d | 2001-08-23 | - | %%g | Week-based year, last two digits (00-99) | 01 | - | %%G | Week-based year | 2001 | - | %%h | Abbreviated month name * (same as %b) | Aug | - | %%H | Hour in 24h format (00-23) | 14 | - | %%I | Hour in 12h format (01-12) | 02 | - | %%j | Day of the year (001-366) | 235 | - | %%m | Month as a decimal number (01-12) | 08 | - | %%M | Minute (00-59) | 55 | - | %%n | New-line character ('\\n') | | - | %%p | AM or PM designation | PM | - | %%r | 12-hour clock time * | 02:55:02 pm | - | %%R | 24-hour HH:MM time, equivalent to %H:%M | 14:55 | - | %%S | Second (00-61) | 02 | - | %%t | Horizontal-tab character ('\\t') | | - | %%T | ISO 8601 time format (HH:MM:SS), equivalent to %H:%M:%S | 14:55:02 | - | %%u | ISO 8601 weekday as number with Monday as 1 (1-7) | 4 | - | %%U | Week number with the first Sunday as the first day of week one (00-53) | 33 | - | %%V | ISO 8601 week number (00-53) | 34 | - | %%w | Weekday as a decimal number with Sunday as 0 (0-6) | 4 | - | %%W | Week number with the first Monday as the first day of week one (00-53) | 34 | - | %%x | Date representation * | 08/23/01 | - | %%X | Time representation * | 14:55:02 | - | %%y | Year, last two digits (00-99) | 01 | - | %%Y | Year | 2001 | - | %%z | ISO 8601 offset from UTC in timezone (1 minute=1, 1 hour=100) \n If timezone cannot be determined, no -characters | +100 | | %%Z | Timezone name or abbreviation * \n If timezone cannot be determined, no -characters| CDT | | %% | A % sign | % | - * The specifiers marked with an asterisk (*) are locale-dependent. - - \return A formated date and time string. When default format is used, the -returned string contains "YYYY/MM/DD hh:mm:ss". - - The following example shows how to use this function: - \code + Return a string containing date and time. + + \param[in] format : The string format supported by strftime() function that represents the time. + The default format is the following \c "%Y/%m/%d %H:%M:%S". + This string contains any combination of special format specifiers given in the next table: + | specifier | Replaced by | Example | + |-----------|------------------------------------------------------|--------------------------| + | %%a | Abbreviated weekday name * | Thu | + | %%A | Full weekday name * | Thursday | + | %%b | Abbreviated month name * | Aug | + | %%B | Full month name * | August | + | %%c | Date and time representation * | Thu Aug 23 14:55:02 2001 | + | %%C | Year divided by 100 and truncated to integer (00-99) | 20 | + | %%d | Day of the month, zero-padded (01-31) | 23 | + | %%D | Short MM/DD/YY date, equivalent to %m/%d/%y | 08/23/01 | + | %%e | Day of the month, space-padded ( 1-31) | 23 | + | %%F | Short YYYY-MM-DD date, equivalent to %Y-%m-%d | 2001-08-23 | + | %%g | Week-based year, last two digits (00-99) | 01 | + | %%G | Week-based year | 2001 | + | %%h | Abbreviated month name * (same as %b) | Aug | + | %%H | Hour in 24h format (00-23) | 14 | + | %%I | Hour in 12h format (01-12) | 02 | + | %%j | Day of the year (001-366) | 235 | + | %%m | Month as a decimal number (01-12) | 08 | + | %%M | Minute (00-59) | 55 | + | %%n | New-line character ('\\n') | | + | %%p | AM or PM designation | PM | + | %%r | 12-hour clock time * | 02:55:02 pm | + | %%R | 24-hour HH:MM time, equivalent to %H:%M | 14:55 | + | %%S | Second (00-61) | 02 | + | %%t | Horizontal-tab character ('\\t') | | + | %%T | ISO 8601 time format (HH:MM:SS), equivalent to %H:%M:%S | 14:55:02 | + | %%u | ISO 8601 weekday as number with Monday as 1 (1-7) | 4 | + | %%U | Week number with the first Sunday as the first day of week one (00-53) | 33 | + | %%V | ISO 8601 week number (00-53) | 34 | + | %%w | Weekday as a decimal number with Sunday as 0 (0-6) | 4 | + | %%W | Week number with the first Monday as the first day of week one (00-53) | 34 | + | %%x | Date representation * | 08/23/01 | + | %%X | Time representation * | 14:55:02 | + | %%y | Year, last two digits (00-99) | 01 | + | %%Y | Year | 2001 | + | %%z | ISO 8601 offset from UTC in timezone (1 minute=1, 1 hour=100) \n If timezone cannot be determined, no +characters | +100 | | %%Z | Timezone name or abbreviation * \n If timezone cannot be determined, no +characters| CDT | | %% | A % sign | % | + * The specifiers marked with an asterisk (*) are locale-dependent. + + \return A formatted date and time string. When default format is used, the + returned string contains "YYYY/MM/DD hh:mm:ss". + + The following example shows how to use this function: + \code #include int main() @@ -371,7 +373,7 @@ std::string getDateTime(const std::string &format) }; #endif -vpChrono::vpChrono() : m_durationMs(), m_lastTimePoint() {} +vpChrono::vpChrono() : m_durationMs(), m_lastTimePoint() { } /*! Get chrono duration in microsecond. diff --git a/modules/core/src/tracking/moments/vpMomentAreaNormalized.cpp b/modules/core/src/tracking/moments/vpMomentAreaNormalized.cpp index 6638fcdcce..e81999de34 100644 --- a/modules/core/src/tracking/moments/vpMomentAreaNormalized.cpp +++ b/modules/core/src/tracking/moments/vpMomentAreaNormalized.cpp @@ -50,11 +50,12 @@ void vpMomentAreaNormalized::compute() bool found_moment_centered; /* getMoments() returns a reference to a vpMomentDatabase. (a protected - member inherited from vpMoment) - .get() is a member function of vpMomentDatabase that returns - a specific moment which is linked to it*/ + * member inherited from vpMoment) + *.get() is a member function of vpMomentDatabase that returns + * a specific moment which is linked to it + * */ const vpMomentCentered &momentCentered = - static_cast(getMoments().get("vpMomentCentered", found_moment_centered)); + static_cast(getMoments().get("vpMomentCentered", found_moment_centered)); if (!found_moment_centered) throw vpException(vpException::notInitialized, "vpMomentCentered not found"); @@ -107,7 +108,7 @@ void vpMomentAreaNormalized::printDependencies(std::ostream &os) const bool found_moment_centered; const vpMomentCentered &momentCentered = - static_cast(getMoments().get("vpMomentCentered", found_moment_centered)); + static_cast(getMoments().get("vpMomentCentered", found_moment_centered)); if (!found_moment_centered) throw vpException(vpException::notInitialized, "vpMomentCentered not found"); diff --git a/modules/core/src/tracking/moments/vpMomentCInvariant.cpp b/modules/core/src/tracking/moments/vpMomentCInvariant.cpp index 5b689a8f1d..782c199e3b 100644 --- a/modules/core/src/tracking/moments/vpMomentCInvariant.cpp +++ b/modules/core/src/tracking/moments/vpMomentCInvariant.cpp @@ -29,7 +29,7 @@ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. * * Description: - * Descriptor for various invariants used to drive space roations around X and + * Descriptor for various invariants used to drive space rotations around X and *Y axis. * * Authors: diff --git a/modules/core/src/tracking/moments/vpMomentDatabase.cpp b/modules/core/src/tracking/moments/vpMomentDatabase.cpp index f475c02b49..12e28b9d96 100644 --- a/modules/core/src/tracking/moments/vpMomentDatabase.cpp +++ b/modules/core/src/tracking/moments/vpMomentDatabase.cpp @@ -63,7 +63,7 @@ void vpMomentDatabase::add(vpMoment &moment, const char *name) */ const vpMoment &vpMomentDatabase::get(const char *type, bool &found) const { - std::map::const_iterator it = moments.find(type); + std::map::const_iterator it = moments.find(type); found = (it != moments.end()); return *(it->second); @@ -81,7 +81,7 @@ const vpMoment &vpMomentDatabase::get(const char *type, bool &found) const */ void vpMomentDatabase::updateAll(vpMomentObject &object) { - std::map::const_iterator itr; + std::map::const_iterator itr; for (itr = moments.begin(); itr != moments.end(); ++itr) { (*itr).second->update(object); } @@ -92,7 +92,7 @@ void vpMomentDatabase::updateAll(vpMomentObject &object) */ VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentDatabase &m) { - std::map::const_iterator itr; + std::map::const_iterator itr; os << "{"; for (itr = m.moments.begin(); itr != m.moments.end(); ++itr) { diff --git a/modules/core/src/tracking/moments/vpMomentObject.cpp b/modules/core/src/tracking/moments/vpMomentObject.cpp index 5ac1aeda22..83974dc4d2 100644 --- a/modules/core/src/tracking/moments/vpMomentObject.cpp +++ b/modules/core/src/tracking/moments/vpMomentObject.cpp @@ -416,7 +416,7 @@ void vpMomentObject::fromImage(const vpImage &image, const vpCame } } } else { - /////////// BLACK BACKGROUND /////////// + /////////// BLACK BACKGROUND /////////// for (unsigned int j = 0; j < image.getRows(); j++) { for (unsigned int i = 0; i < image.getCols(); i++) { x = 0; diff --git a/modules/core/test/camera/testJsonCamera.cpp b/modules/core/test/camera/testJsonCamera.cpp index e5dd8d71e8..61207109a5 100644 --- a/modules/core/test/camera/testJsonCamera.cpp +++ b/modules/core/test/camera/testJsonCamera.cpp @@ -44,7 +44,7 @@ #if defined(VISP_HAVE_NLOHMANN_JSON) && defined(VISP_HAVE_CATCH2) #include -using json = nlohmann::json; +using json = nlohmann::json; //! json namespace shortcut #define CATCH_CONFIG_RUNNER #include @@ -53,7 +53,7 @@ using json = nlohmann::json; namespace { // This class shows how to implement a simple generator for Catch tests -class RandomCamGenerator : public Catch::Generators::IGenerator +class vpRandomCamGenerator : public Catch::Generators::IGenerator { private: std::minstd_rand m_rand; @@ -64,7 +64,7 @@ class RandomCamGenerator : public Catch::Generators::IGenerator(next()); @@ -108,7 +108,7 @@ class RandomCamGenerator : public Catch::Generators::IGenerator randomCam(double low, double high) { return Catch::Generators::GeneratorWrapper( - std::unique_ptr >(new RandomCamGenerator(low, high))); + std::unique_ptr >(new vpRandomCamGenerator(low, high))); } } // namespace diff --git a/modules/core/test/image-with-dataset/testConversion.cpp b/modules/core/test/image-with-dataset/testConversion.cpp index 4bff650e33..a8fee2ac6b 100644 --- a/modules/core/test/image-with-dataset/testConversion.cpp +++ b/modules/core/test/image-with-dataset/testConversion.cpp @@ -72,7 +72,7 @@ Test image conversions.\n\ \n\ SYNOPSIS\n\ %s [-i ] [-o ] [-n ]\n\ - [-h]\n \ + [-h]\n\ ", name); diff --git a/modules/core/test/image-with-dataset/testCrop.cpp b/modules/core/test/image-with-dataset/testCrop.cpp index 56fc258c64..9f64551516 100644 --- a/modules/core/test/image-with-dataset/testCrop.cpp +++ b/modules/core/test/image-with-dataset/testCrop.cpp @@ -76,7 +76,7 @@ and save the cropped image on the disk (Klimt_cropped.pgm).\n\ \n\ SYNOPSIS\n\ %s [-i ] [-o ]\n\ - [-h]\n \ + [-h]\n\ ", name); diff --git a/modules/core/test/image-with-dataset/testIoEXR.cpp b/modules/core/test/image-with-dataset/testIoEXR.cpp index 6fe9d4653c..ead98edbe9 100644 --- a/modules/core/test/image-with-dataset/testIoEXR.cpp +++ b/modules/core/test/image-with-dataset/testIoEXR.cpp @@ -78,77 +78,79 @@ void checkGrayImages(const vpImage &I1, const vpImage &I2, #endif } // namespace -TEST_CASE("EXR image read", "[exr_image_io]"){ +TEST_CASE("EXR image read", "[exr_image_io]") +{ // Disable the tests if big endian for now. // See: https://github.com/syoyo/tinyexr/issues/189#issuecomment-1465174904 #ifdef VISP_LITTLE_ENDIAN - SECTION("Color"){const std::string imgPathRef = vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), - "memorial/memorial_color_LSB.pfm"); -REQUIRE(vpIoTools::checkFilename(imgPathRef)); + SECTION("Color") + { + const std::string imgPathRef = vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "memorial/memorial_color_LSB.pfm"); + REQUIRE(vpIoTools::checkFilename(imgPathRef)); -vpImage I_ref; -vpImageIo::readPFM_HDR(I_ref, imgPathRef); -CHECK(I_ref.getSize() > 0); + vpImage I_ref; + vpImageIo::readPFM_HDR(I_ref, imgPathRef); + CHECK(I_ref.getSize() > 0); -SECTION("32-bits") -{ - const std::string imgPath = - vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "memorial/memorial_color_32bits.exr"); - REQUIRE(vpIoTools::checkFilename(imgPath)); + SECTION("32-bits") + { + const std::string imgPath = + vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "memorial/memorial_color_32bits.exr"); + REQUIRE(vpIoTools::checkFilename(imgPath)); - vpImage I; - vpImageIo::readEXR(I, imgPath); - CHECK(I.getSize() > 0); - checkColorImages(I_ref, I); -} + vpImage I; + vpImageIo::readEXR(I, imgPath); + CHECK(I.getSize() > 0); + checkColorImages(I_ref, I); + } -SECTION("16-bits") -{ - const std::string imgPath = - vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "memorial/memorial_color_16bits.exr"); - REQUIRE(vpIoTools::checkFilename(imgPath)); - - vpImage I; - vpImageIo::readEXR(I, imgPath); - CHECK(I.getSize() > 0); - checkColorImages(I_ref, I, 0.00097656f); -} -} + SECTION("16-bits") + { + const std::string imgPath = + vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "memorial/memorial_color_16bits.exr"); + REQUIRE(vpIoTools::checkFilename(imgPath)); -SECTION("Gray") -{ - const std::string imgPathRef = + vpImage I; + vpImageIo::readEXR(I, imgPath); + CHECK(I.getSize() > 0); + checkColorImages(I_ref, I, 0.00097656f); + } + } + + SECTION("Gray") + { + const std::string imgPathRef = vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "memorial/memorial_gray_LSB.pfm"); - REQUIRE(vpIoTools::checkFilename(imgPathRef)); + REQUIRE(vpIoTools::checkFilename(imgPathRef)); - vpImage I_ref; - vpImageIo::readPFM_HDR(I_ref, imgPathRef); - CHECK(I_ref.getSize() > 0); + vpImage I_ref; + vpImageIo::readPFM_HDR(I_ref, imgPathRef); + CHECK(I_ref.getSize() > 0); - SECTION("32-bits") - { - const std::string imgPath = + SECTION("32-bits") + { + const std::string imgPath = vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "memorial/memorial_gray_32bits.exr"); - REQUIRE(vpIoTools::checkFilename(imgPath)); + REQUIRE(vpIoTools::checkFilename(imgPath)); - vpImage I; - vpImageIo::readEXR(I, imgPath); - CHECK(I.getSize() > 0); - checkGrayImages(I_ref, I); - } + vpImage I; + vpImageIo::readEXR(I, imgPath); + CHECK(I.getSize() > 0); + checkGrayImages(I_ref, I); + } - SECTION("16-bits") - { - const std::string imgPath = + SECTION("16-bits") + { + const std::string imgPath = vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "memorial/memorial_gray_16bits.exr"); - REQUIRE(vpIoTools::checkFilename(imgPath)); + REQUIRE(vpIoTools::checkFilename(imgPath)); - vpImage I; - vpImageIo::readEXR(I, imgPath); - CHECK(I.getSize() > 0); - checkGrayImages(I_ref, I, 0.00097656f); + vpImage I; + vpImageIo::readEXR(I, imgPath); + CHECK(I.getSize() > 0); + checkGrayImages(I_ref, I, 0.00097656f); + } } -} #endif } @@ -163,7 +165,7 @@ TEST_CASE("EXR image write", "[exr_image_io]") SECTION("Color") { const std::string imgPath = - vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "memorial/memorial_color_32bits.exr"); + vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "memorial/memorial_color_32bits.exr"); REQUIRE(vpIoTools::checkFilename(imgPath)); vpImage I; @@ -183,7 +185,7 @@ TEST_CASE("EXR image write", "[exr_image_io]") SECTION("Gray") { const std::string imgPath = - vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "memorial/memorial_gray_32bits.exr"); + vpIoTools::createFilePath(vpIoTools::getViSPImagesDataPath(), "memorial/memorial_gray_32bits.exr"); REQUIRE(vpIoTools::checkFilename(imgPath)); vpImage I; diff --git a/modules/core/test/image-with-dataset/testIoPGM.cpp b/modules/core/test/image-with-dataset/testIoPGM.cpp index ffc441eab4..a1077b1b29 100644 --- a/modules/core/test/image-with-dataset/testIoPGM.cpp +++ b/modules/core/test/image-with-dataset/testIoPGM.cpp @@ -70,7 +70,7 @@ Read and write PGM images on the disk. Also test exceptions.\n\ \n\ SYNOPSIS\n\ %s [-i ] [-o ]\n\ - [-h]\n \ + [-h]\n\ ", name); diff --git a/modules/core/test/image-with-dataset/testIoPPM.cpp b/modules/core/test/image-with-dataset/testIoPPM.cpp index a3223d8c65..d4195abfa9 100644 --- a/modules/core/test/image-with-dataset/testIoPPM.cpp +++ b/modules/core/test/image-with-dataset/testIoPPM.cpp @@ -71,7 +71,7 @@ Read and write PPM images on the disk. Also test exceptions.\n\ \n\ SYNOPSIS\n\ %s [-i ] [-o ]\n\ - [-h]\n \ + [-h]\n\ ", name); diff --git a/modules/core/test/image-with-dataset/testReadImage.cpp b/modules/core/test/image-with-dataset/testReadImage.cpp index 47de5bfe42..0e5325085d 100644 --- a/modules/core/test/image-with-dataset/testReadImage.cpp +++ b/modules/core/test/image-with-dataset/testReadImage.cpp @@ -70,7 +70,7 @@ Read images on the disk.\n\ \n\ SYNOPSIS\n\ %s [-i ] [-p ]\n\ - [-h]\n \ + [-h]\n\ ", name); diff --git a/modules/core/test/math/perfColVectorOperations.cpp b/modules/core/test/math/perfColVectorOperations.cpp index 6ca204e13d..c67a0790ec 100644 --- a/modules/core/test/math/perfColVectorOperations.cpp +++ b/modules/core/test/math/perfColVectorOperations.cpp @@ -45,7 +45,7 @@ namespace { static bool g_runBenchmark = false; -static const std::vector g_sizes = {23, 127, 233, 419, 1153, 2749}; +static const std::vector g_sizes = { 23, 127, 233, 419, 1153, 2749 }; double getRandomValues(double min, double max) { return (max - min) * ((double)rand() / (double)RAND_MAX) + min; } @@ -66,7 +66,8 @@ double stddev(const std::vector &vec) double mean = sum / vec.size(); std::vector diff(vec.size()); - std::transform(vec.begin(), vec.end(), diff.begin(), [mean](double x) { return x - mean; }); + std::transform(vec.begin(), vec.end(), diff.begin(), [mean](double x) { + return x - mean; }); double sq_sum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0); return std::sqrt(sq_sum / vec.size()); } @@ -369,11 +370,11 @@ int main(int argc, char *argv[]) // Build a new parser on top of Catch's using namespace Catch::clara; auto cli = session.cli() // Get Catch's composite command line parser - | Opt(g_runBenchmark) // bind variable to a new option, with a hint string - ["--benchmark"] // the option names it will respond to - ("run benchmark?"); // description string for the help output + | Opt(g_runBenchmark) // bind variable to a new option, with a hint string + ["--benchmark"] // the option names it will respond to + ("run benchmark?"); // description string for the help output - // Now pass the new composite back to Catch so it uses that +// Now pass the new composite back to Catch so it uses that session.cli(cli); // Let Catch (using Clara) parse the command line diff --git a/modules/core/test/math/testJsonArrayConversion.cpp b/modules/core/test/math/testJsonArrayConversion.cpp index 67203e1d7d..c1d51a1361 100644 --- a/modules/core/test/math/testJsonArrayConversion.cpp +++ b/modules/core/test/math/testJsonArrayConversion.cpp @@ -48,7 +48,7 @@ #include #include -using json = nlohmann::json; +using json = nlohmann::json; //! json namespace shortcut #define CATCH_CONFIG_RUNNER #include "catch.hpp" @@ -79,7 +79,7 @@ class vpExceptionMatcher : public Catch::Matchers::Impl::MatcherBase > +class vpRandomArray2DGenerator : public Catch::Generators::IGenerator > { private: std::minstd_rand m_rand; @@ -89,8 +89,8 @@ class RandomArray2DGenerator : public Catch::Generators::IGenerator current; public: - RandomArray2DGenerator(double valueRange, int minSize, int maxSize) - : m_rand(std::random_device{}()), m_val_dist(-valueRange, valueRange), m_dim_dist(minSize, maxSize) + vpRandomArray2DGenerator(double valueRange, int minSize, int maxSize) + : m_rand(std::random_device {}()), m_val_dist(-valueRange, valueRange), m_dim_dist(minSize, maxSize) { static_cast(next()); @@ -110,7 +110,7 @@ class RandomArray2DGenerator : public Catch::Generators::IGenerator +class vpRandomColVectorGenerator : public Catch::Generators::IGenerator { private: std::minstd_rand m_rand; @@ -120,8 +120,8 @@ class RandomColVectorGenerator : public Catch::Generators::IGenerator(next()); @@ -142,12 +142,12 @@ Catch::Generators::GeneratorWrapper > randomArray(double v, in { return Catch::Generators::GeneratorWrapper >( std::unique_ptr > >( - new RandomArray2DGenerator(v, minSize, maxSize))); + new vpRandomArray2DGenerator(v, minSize, maxSize))); } Catch::Generators::GeneratorWrapper randomColVector(double v, int minSize, int maxSize) { return Catch::Generators::GeneratorWrapper( - std::unique_ptr >(new RandomColVectorGenerator(v, minSize, maxSize))); + std::unique_ptr >(new vpRandomColVectorGenerator(v, minSize, maxSize))); } vpExceptionMatcher matchVpException(vpException::generalExceptionEnum type, const StringMatcherBase &matcher) { @@ -326,7 +326,7 @@ SCENARIO("Serializing and deserializing a vpColVector", "[json]") } } -int main(int argc, char *argv []) +int main(int argc, char *argv[]) { Catch::Session session; // There must be exactly one instance session.applyCommandLine(argc, argv); diff --git a/modules/core/test/math/testRotation.cpp b/modules/core/test/math/testRotation.cpp index 0d76f1fd45..8f8d0d397a 100644 --- a/modules/core/test/math/testRotation.cpp +++ b/modules/core/test/math/testRotation.cpp @@ -425,13 +425,31 @@ int main() { // Test rotation_matrix * homogeneous_matrix vpHomogeneousMatrix _1_M_2_truth; - _1_M_2_truth[0][0] = 0.9835; _1_M_2_truth[0][1] = -0.0581; _1_M_2_truth[0][2] = 0.1716; _1_M_2_truth[0][3] = 0; - _1_M_2_truth[1][0] = -0.0489; _1_M_2_truth[1][1] = -0.9972; _1_M_2_truth[1][2] = -0.0571; _1_M_2_truth[1][3] = 0; - _1_M_2_truth[2][0] = 0.1744; _1_M_2_truth[2][1] = 0.0478; _1_M_2_truth[2][2] = -0.9835; _1_M_2_truth[2][3] = 0; + _1_M_2_truth[0][0] = 0.9835; + _1_M_2_truth[0][1] = -0.0581; + _1_M_2_truth[0][2] = 0.1716; + _1_M_2_truth[0][3] = 0; + _1_M_2_truth[1][0] = -0.0489; + _1_M_2_truth[1][1] = -0.9972; + _1_M_2_truth[1][2] = -0.0571; + _1_M_2_truth[1][3] = 0; + _1_M_2_truth[2][0] = 0.1744; + _1_M_2_truth[2][1] = 0.0478; + _1_M_2_truth[2][2] = -0.9835; + _1_M_2_truth[2][3] = 0; vpHomogeneousMatrix _2_M_3_; - _2_M_3_[0][0] = 0.9835; _2_M_3_[0][1] = -0.0581; _2_M_3_[0][2] = 0.1716; _2_M_3_[0][3] = 0.0072; - _2_M_3_[1][0] = -0.0489; _2_M_3_[1][1] = -0.9972; _2_M_3_[1][2] = -0.0571; _2_M_3_[1][3] = 0.0352; - _2_M_3_[2][0] = 0.1744; _2_M_3_[2][1] = 0.0478; _2_M_3_[2][2] = -0.9835; _2_M_3_[2][3] = 0.9470; + _2_M_3_[0][0] = 0.9835; + _2_M_3_[0][1] = -0.0581; + _2_M_3_[0][2] = 0.1716; + _2_M_3_[0][3] = 0.0072; + _2_M_3_[1][0] = -0.0489; + _2_M_3_[1][1] = -0.9972; + _2_M_3_[1][2] = -0.0571; + _2_M_3_[1][3] = 0.0352; + _2_M_3_[2][0] = 0.1744; + _2_M_3_[2][1] = 0.0478; + _2_M_3_[2][2] = -0.9835; + _2_M_3_[2][3] = 0.9470; vpRotationMatrix _1_R_2_ = _1_M_2_truth.getRotationMatrix(); vpHomogeneousMatrix _1_M_3_(_1_R_2_* _2_M_3_); diff --git a/modules/core/test/network/testUDPClient.cpp b/modules/core/test/network/testUDPClient.cpp index 9262e1bbfe..de36ca400b 100644 --- a/modules/core/test/network/testUDPClient.cpp +++ b/modules/core/test/network/testUDPClient.cpp @@ -46,12 +46,12 @@ namespace { -struct DataType { +struct vpDataType_t { double double_val; int int_val; - DataType() : double_val(0.0), int_val(0) {} - DataType(double dbl, int i) : double_val(dbl), int_val(i) {} + vpDataType_t() : double_val(0.0), int_val(0) {} + vpDataType_t(double dbl, int i) : double_val(dbl), int_val(i) {} }; } // namespace @@ -76,7 +76,7 @@ int main(int argc, char **argv) vpUDPClient client(servername, port); // Send custom data type - DataType data_type(1234.56789, 123450); + vpDataType_t data_type(1234.56789, 123450); char data[sizeof(data_type.double_val) + sizeof(data_type.int_val)]; memcpy(data, &data_type.double_val, sizeof(data_type.double_val)); memcpy(data + sizeof(data_type.double_val), &data_type.int_val, sizeof(data_type.int_val)); diff --git a/modules/core/test/network/testUDPServer.cpp b/modules/core/test/network/testUDPServer.cpp index 9705ce84ce..c5afecf488 100644 --- a/modules/core/test/network/testUDPServer.cpp +++ b/modules/core/test/network/testUDPServer.cpp @@ -49,12 +49,12 @@ namespace { -struct DataType { +struct vpDataType_t { double double_val; int int_val; - DataType() : double_val(0.0), int_val(0) {} - DataType(double dbl, int i) : double_val(dbl), int_val(i) {} + vpDataType_t() : double_val(0.0), int_val(0) {} + vpDataType_t(double dbl, int i) : double_val(dbl), int_val(i) {} }; } // namespace @@ -70,7 +70,7 @@ int main() // Receive and send custom data type int res = server.receive(msg, hostInfo); if (res) { - DataType data_type; + vpDataType_t data_type; memcpy(&data_type.double_val, msg.c_str(), sizeof(data_type.double_val)); memcpy(&data_type.int_val, msg.c_str() + sizeof(data_type.double_val), sizeof(data_type.int_val)); std::cout << "Server received double_val: " << data_type.double_val << " ; int_val: " << data_type.int_val diff --git a/modules/core/test/tools/geometry/testPolygon.cpp b/modules/core/test/tools/geometry/testPolygon.cpp index 25cc42fa49..132eda4f40 100644 --- a/modules/core/test/tools/geometry/testPolygon.cpp +++ b/modules/core/test/tools/geometry/testPolygon.cpp @@ -68,7 +68,7 @@ void usage(const char *name, const char *badparam) test the generic 2D polygons.\n\ \n\ SYNOPSIS\n\ - %s [-c] [-d] [-h]\n \ + %s [-c] [-d] [-h]\n\ ", name); diff --git a/modules/core/test/tools/threading/testThread2.cpp b/modules/core/test/tools/threading/testThread2.cpp index 0c44a0dd8e..5fba1d2687 100644 --- a/modules/core/test/tools/threading/testThread2.cpp +++ b/modules/core/test/tools/threading/testThread2.cpp @@ -54,15 +54,14 @@ namespace { //! [functor-thread-example declaration] -class ArithmFunctor +class vpArithmFunctor { public: - ArithmFunctor(const vpColVector &v1, const vpColVector &v2, unsigned int start, unsigned int end) + vpArithmFunctor(const vpColVector &v1, const vpColVector &v2, unsigned int start, unsigned int end) : m_add(), m_mul(), m_v1(v1), m_v2(v2), m_indexStart(start), m_indexEnd(end) - { - } + { } - ArithmFunctor() : m_add(), m_mul(), m_v1(), m_v2(), m_indexStart(0), m_indexEnd(0) {} + vpArithmFunctor() : m_add(), m_mul(), m_v1(), m_v2(), m_indexStart(0), m_indexEnd(0) { } void operator()() { computeImpl(); } @@ -97,7 +96,7 @@ class ArithmFunctor //! [functor-thread-example threadFunction] vpThread::Return arithmThread(vpThread::Args args) { - ArithmFunctor *f = static_cast(args); + vpArithmFunctor *f = static_cast(args); (*f)(); return 0; } @@ -155,13 +154,14 @@ int main() //! [functor-thread-example threadCreation] std::vector threads(nb_threads); - std::vector functors(nb_threads); + std::vector functors(nb_threads); unsigned int split = size / nb_threads; for (unsigned int i = 0; i < nb_threads; i++) { if (i < nb_threads - 1) { - functors[i] = ArithmFunctor(v1, v2, i * split, (i + 1) * split); - } else { - functors[i] = ArithmFunctor(v1, v2, i * split, size); + functors[i] = vpArithmFunctor(v1, v2, i * split, (i + 1) * split); + } + else { + functors[i] = vpArithmFunctor(v1, v2, i * split, size); } std::cout << "Create thread: " << i << std::endl; diff --git a/modules/detection/include/visp3/detection/vpDetectorDNNOpenCV.h b/modules/detection/include/visp3/detection/vpDetectorDNNOpenCV.h index 5a3a4b79f1..e0010b5367 100644 --- a/modules/detection/include/visp3/detection/vpDetectorDNNOpenCV.h +++ b/modules/detection/include/visp3/detection/vpDetectorDNNOpenCV.h @@ -52,7 +52,7 @@ #include #ifdef VISP_HAVE_NLOHMANN_JSON #include -using json = nlohmann::json; +using json = nlohmann::json; //! json namespace shortcut #endif /*! diff --git a/modules/gui/src/display/vpDisplayGTK.cpp b/modules/gui/src/display/vpDisplayGTK.cpp index a8cb38e711..a920903096 100644 --- a/modules/gui/src/display/vpDisplayGTK.cpp +++ b/modules/gui/src/display/vpDisplayGTK.cpp @@ -474,15 +474,10 @@ class vpDisplayGTK::Impl GdkEvent *ev = NULL; while ((ev = gdk_event_get()) != NULL) { cpt++; - // printf("event %d type %d on window %p My window %p\n", - // cpt, ev->type, ev->any.window, widget->window); if (ev->any.window == m_widget->window && ev->type == GDK_KEY_PRESS) { - // std::cout << "Key val: \"" << gdk_keyval_name (ev->key.keyval) - // /*ev->key.string*/ << "\"" << std::endl; key = gdk_keyval_name(ev->key.keyval); ret = true; - // printf("Key press detection\n"); } gdk_event_free(ev); } diff --git a/modules/gui/src/display/windows/vpGDIRenderer.cpp b/modules/gui/src/display/windows/vpGDIRenderer.cpp index 1ee98f6ec2..b985f795e7 100644 --- a/modules/gui/src/display/windows/vpGDIRenderer.cpp +++ b/modules/gui/src/display/windows/vpGDIRenderer.cpp @@ -886,8 +886,7 @@ void vpGDIRenderer::drawCross(const vpImagePoint &ip, unsigned int size, const v LineTo(hDCMem, vpMath::round(ip.get_u() / m_rscale), vpMath::round(ip.get_v() / m_rscale) + half_size); // display the result (flush) - // BitBlt(hDCScreen, j-(size/2), i-(size/2), size, size, - // hDCMem, j-(size/2), i-(size/2), SRCCOPY); + // BitBlt(hDCScreen, j-(size/2), i-(size/2), size, size, hDCMem, j-(size/2), i-(size/2), SRCCOPY); LeaveCriticalSection(&m_criticalSection); diff --git a/modules/gui/src/plot/vpPlot.cpp b/modules/gui/src/plot/vpPlot.cpp index c0a629b682..fdfc1db1c4 100644 --- a/modules/gui/src/plot/vpPlot.cpp +++ b/modules/gui/src/plot/vpPlot.cpp @@ -196,29 +196,20 @@ void vpPlot::initNbGraph(unsigned int nbGraph) \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 - and 3. \param curveNbr : The number of curves belonging to the graphic + and 3. + \param curveNbr : The number of curves belonging to the graphic */ void vpPlot::initGraph(unsigned int graphNum, unsigned int curveNbr) { (graphList + graphNum)->initGraph(curveNbr); } -// void -// vpPlot::initRange (const int graphNum, -// double xmin, double xmax, double /*xdelt*/, -// double ymin, double ymax, double /*ydelt*/, -// bool gx, bool gy) -// { -// (graphList+graphNum)->initScale(I,xmin,xmax,10,ymin,ymax,10,gx,gy); -// } - /*! This method enables to set the initial range of the selected graphic. \param graphNum : The index of the graph in the window. As the number of - graphic in a window is less or equal to 4, this parameter is between 0 - and 3. \param xmin : The initial minimum value along the x axis given in the - user coordinates. \param xmax : The initial maximum value along the x axis - given in the user coordinates. \param ymin : The initial minimum value along - the y axis given in the user coordinates. \param ymax : The initial maximum - value along the y axis given in the user coordinates. + graphic in a window is less or equal to 4, this parameter is between 0 and 3. + \param xmin : The initial minimum value along the x axis given in the user coordinates. + \param xmax : The initial maximum value along the x axis given in the user coordinates. + \param ymin : The initial minimum value along the y axis given in the user coordinates. + \param ymax : The initial maximum value along the y axis given in the user coordinates. */ void vpPlot::initRange(unsigned int graphNum, double xmin, double xmax, double ymin, double ymax) { @@ -229,15 +220,13 @@ void vpPlot::initRange(unsigned int graphNum, double xmin, double xmax, double y This method enables to set the initial range of the selected graphic. \param graphNum : The index of the graph in the window. As the number of - graphic in a window is less or equal to 4, this parameter is between 0 - and 3. \param xmin : The initial minimum value along the x axis given in the - user coordinates. \param xmax : The initial maximum value along the x axis - given in the user coordinates. \param ymin : The initial minimum value along - the y axis given in the user coordinates. \param ymax : The initial maximum - value along the y axis given in the user coordinates. \param zmin : The - initial minimum value along the z axis given in the user coordinates. \param - zmax : The initial maximum value along the z axis given in the user - coordinates. + graphic in a window is less or equal to 4, this parameter is between 0 and 3. + \param xmin : The initial minimum value along the x axis given in the user coordinates. + \param xmax : The initial maximum value along the x axis given in the user coordinates. + \param ymin : The initial minimum value along the y axis given in the user coordinates. + \param ymax : The initial maximum value along the y axis given in the user coordinates. + \param zmin : The initial minimum value along the z axis given in the user coordinates. + \param zmax : The initial maximum value along the z axis given in the user coordinates. */ void vpPlot::initRange(unsigned int graphNum, double xmin, double xmax, double ymin, double ymax, double zmin, double zmax) @@ -249,9 +238,9 @@ void vpPlot::initRange(unsigned int graphNum, double xmin, double xmax, double y This function enables you to choose the color used to draw a given curve. \param graphNum : The index of the graph in the window. As the number of - graphic in a window is less or equal to 4, this parameter is between 0 - and 3. \param curveNum : The index of the curve in the list of the curves - belonging to the graphic. \param color : The color you want to use + graphic in a window is less or equal to 4, this parameter is between 0 and 3. + \param curveNum : The index of the curve in the list of the curves belonging to the graphic. + \param color : The color you want to use */ void vpPlot::setColor(unsigned int graphNum, unsigned int curveNum, vpColor color) { @@ -272,11 +261,10 @@ void vpPlot::displayGrid() drawn with the parameters of the curve. \param graphNum : The index of the graph in the window. As the number of - graphic in a window is less or equal to 4, this parameter is between 0 - and 3. \param curveNum : The index of the curve in the list of the curves - belonging to the graphic. \param x : The coordinate of the new point along - the x axis and given in the user unit system. \param y : The coordinate of - the new point along the y axis and given in the user unit system. + graphic in a window is less or equal to 4, this parameter is between 0 and 3. + \param curveNum : The index of the curve in the list of the curves belonging to the graphic. + \param x : The coordinate of the new point along the x axis and given in the user unit system. + \param y : The coordinate of the new point along the y axis and given in the user unit system. */ void vpPlot::plot(unsigned int graphNum, unsigned int curveNum, double x, double y) { @@ -288,11 +276,11 @@ void vpPlot::plot(unsigned int graphNum, unsigned int curveNum, double x, double points are drawn with the parameters of the curves. \param graphNum : The index of the graph in the window. As the number of - graphic in a window is less or equal to 4, this parameter is between 0 - and 3. \param x : The coordinate of the new points along the x axis and - given in the user unit system. \param v_y : y coordinates vector. The - coordinates of the new points along the y axis and given in the user unit - system. + graphic in a window is less or equal to 4, this parameter is between 0 and 3. + \param x : The coordinate of the new points along the x axis and + given in the user unit system. + \param v_y : y coordinates vector. The coordinates of the new points along the + y axis and given in the user unit system. */ void vpPlot::plot(unsigned int graphNum, double x, const vpColVector &v_y) { @@ -309,10 +297,11 @@ void vpPlot::plot(unsigned int graphNum, double x, const vpColVector &v_y) \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 - and 3. \param x : The coordinate of the new points along the x axis and - given in the user unit system. \param v_y : y coordinates vector. The - coordinates of the new points along the y axis and given in the user unit - system. + and 3. + \param x : The coordinate of the new points along the x axis and + given in the user unit system. + \param v_y : y coordinates vector. The coordinates of the new points along + the y axis and given in the user unit system. */ void vpPlot::plot(unsigned int graphNum, double x, const vpRowVector &v_y) { @@ -330,8 +319,10 @@ void vpPlot::plot(unsigned int graphNum, double x, const vpRowVector &v_y) \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 - and 3. \param x : The coordinate of the new points along the x axis and - given in the user unit system. \param v_y : y coordinates vector. The + and 3. + \param x : The coordinate of the new points along the x axis and + given in the user unit system. + \param v_y : y coordinates vector. The coordinates of the new points along the y axis and given in the user unit system. */ @@ -350,8 +341,10 @@ void vpPlot::plot(unsigned int graphNum, double x, const vpPoseVector &v_y) \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 - and 3. \param x : The coordinate of the new points along the x axis and - given in the user unit system. \param v_y : y coordinates vector. The + and 3. + \param x : The coordinate of the new points along the x axis and + given in the user unit system. + \param v_y : y coordinates vector. The coordinates of the new points along the y axis and given in the user unit system. */ @@ -371,8 +364,10 @@ void vpPlot::plot(unsigned int graphNum, double x, const vpTranslationVector &v_ \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 - and 3. \param x : The coordinate of the new points along the x axis and - given in the user unit system. \param v_y : y coordinates vector. The + and 3. + \param x : The coordinate of the new points along the x axis and + given in the user unit system. + \param v_y : y coordinates vector. The coordinates of the new points along the y axis and given in the user unit system. */ @@ -392,10 +387,14 @@ void vpPlot::plot(unsigned int graphNum, double x, const vpRotationVector &v_y) \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 - and 3. \param curveNum : The index of the curve in the list of the curves - belonging to the graphic. \param x : The coordinate of the new point along - the x axis and given in the user unit system. \param y : The coordinate of - the new point along the y axis and given in the user unit system. \param z : + and 3. + \param curveNum : The index of the curve in the list of the curves + belonging to the graphic. + \param x : The coordinate of the new point along + the x axis and given in the user unit system. + \param y : The coordinate of + the new point along the y axis and given in the user unit system. + \param z : The coordinate of the new point along the z axis and given in the user unit system. */ @@ -411,10 +410,13 @@ vpMouseButton::vpMouseButtonType vpPlot::plot(unsigned int graphNum, unsigned in \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 - and 3. \param x : The coordinate of the new points along the x axis and - given in the user unit system. \param v_y : y coordinates vector. The + and 3. + \param x : The coordinate of the new points along the x axis and + given in the user unit system. + \param v_y : y coordinates vector. The coordinates of the new points along the y axis and given in the user unit - system. \param v_z : z coordinates vector. The coordinates of the new points + system. + \param v_z : z coordinates vector. The coordinates of the new points along the z axis and given in the user unit system. */ vpMouseButton::vpMouseButtonType vpPlot::plot(unsigned int graphNum, double x, const vpColVector &v_y, @@ -435,7 +437,7 @@ vpMouseButton::vpMouseButtonType vpPlot::plot(unsigned int graphNum, double x, c 3D graphic. The navigation is performed using the mouse. - A click on left mouse button allows rotations - A click on middle mouse button allows zoom - - A click on rigt mouse button quit the infinite navigation loop. + - A click on right mouse button quit the infinite navigation loop. */ void vpPlot::navigate() { @@ -495,7 +497,8 @@ void vpPlot::getPixelValue(bool block) \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 - and 3. \param title : The graphic title. + and 3. + \param title : The graphic title. */ void vpPlot::setTitle(unsigned int graphNum, const std::string &title) { (graphList + graphNum)->setTitle(title); } @@ -504,7 +507,8 @@ void vpPlot::setTitle(unsigned int graphNum, const std::string &title) { (graphL \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 - and 3. \param unitx : The name of the unit of the x axis. + and 3. + \param unitx : The name of the unit of the x axis. */ void vpPlot::setUnitX(unsigned int graphNum, const std::string &unitx) { (graphList + graphNum)->setUnitX(unitx); } @@ -513,7 +517,8 @@ void vpPlot::setUnitX(unsigned int graphNum, const std::string &unitx) { (graphL \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 - and 3. \param unity : The name of the unit of the y axis. + and 3. + \param unity : The name of the unit of the y axis. */ void vpPlot::setUnitY(unsigned int graphNum, const std::string &unity) { (graphList + graphNum)->setUnitY(unity); } @@ -522,7 +527,8 @@ void vpPlot::setUnitY(unsigned int graphNum, const std::string &unity) { (graphL \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 - and 3. \param unitz : The name of the unit of the z axis. + and 3. + \param unitz : The name of the unit of the z axis. */ void vpPlot::setUnitZ(unsigned int graphNum, const std::string &unitz) { (graphList + graphNum)->setUnitZ(unitz); } @@ -531,8 +537,10 @@ void vpPlot::setUnitZ(unsigned int graphNum, const std::string &unitz) { (graphL \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 - and 3. \param curveNum : The index of the curve in the list of the curves - belonging to the graphic. \param legend : The legend of the curve. + and 3. + \param curveNum : The index of the curve in the list of the curves + belonging to the graphic. + \param legend : The legend of the curve. */ void vpPlot::setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend) { @@ -554,12 +562,13 @@ void vpPlot::resetPointList(unsigned int graphNum) } /*! -This function enables you to choose the thickness used to draw a given curve. + This function enables you to choose the thickness used to draw a given curve. \param graphNum : The index of the graph in the window. As the number of -graphic in a window is less or equal to 4, this parameter is between 0 and 3. + graphic in a window is less or equal to 4, this parameter is between 0 and 3. \param curveNum : The index of the curve in the list of the curves belonging -to the graphic. \param thickness : The thickness you want to use + to the graphic. + \param thickness : The thickness you want to use */ void vpPlot::setThickness(unsigned int graphNum, unsigned int curveNum, unsigned int thickness) { @@ -567,11 +576,11 @@ void vpPlot::setThickness(unsigned int graphNum, unsigned int curveNum, unsigned } /*! -This function enables you to choose the thickness used to draw all the curves -belonging to a given graphic. + This function enables you to choose the thickness used to draw all the curves + belonging to a given graphic. \param graphNum : The index of the graph in the window. As the number of -graphic in a window is less or equal to 4, this parameter is between 0 and 3. + graphic in a window is less or equal to 4, this parameter is between 0 and 3. \param thickness : The thickness you want to use */ void vpPlot::setGraphThickness(unsigned int graphNum, unsigned int thickness) @@ -586,7 +595,8 @@ void vpPlot::setGraphThickness(unsigned int graphNum, unsigned int thickness) \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 - and 3. \param thickness : The thickness you want to use + and 3. + \param thickness : The thickness you want to use */ void vpPlot::setGridThickness(unsigned int graphNum, unsigned int thickness) { @@ -599,7 +609,8 @@ void vpPlot::setGridThickness(unsigned int graphNum, unsigned int thickness) \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 - and 3. \param curveNum : The index of the curve in the list of the curves + and 3. + \param curveNum : The index of the curve in the list of the curves belonging to the graphic. */ void vpPlot::resetPointList(unsigned int graphNum, unsigned int curveNum) @@ -625,13 +636,14 @@ void vpPlot::resetPointList(unsigned int graphNum, unsigned int curveNum) The columns are delimited thanks to tabulations. - \param title_prefix : Prefix introducted in the first line of the saved + \param title_prefix : Prefix introduced in the first line of the saved file. To exploit a posteriori the resulting curves: - with gnuplot, set title_prefix to "# ". - with Matlab, set title_prefix to "% ". \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 - and 3. \param dataFile : Name of the text file. + and 3. + \param dataFile : Name of the text file. */ void vpPlot::saveData(unsigned int graphNum, const std::string &dataFile, const std::string &title_prefix) { diff --git a/modules/gui/test/display-with-dataset/testMouseEvent.cpp b/modules/gui/test/display-with-dataset/testMouseEvent.cpp index 9273bf7d64..cd4432532c 100644 --- a/modules/gui/test/display-with-dataset/testMouseEvent.cpp +++ b/modules/gui/test/display-with-dataset/testMouseEvent.cpp @@ -115,7 +115,7 @@ to a PGM file.\n\ SYNOPSIS\n\ %s [-i ] [-p ]\n\ [-f ] [-l ] [-s ] \n\ - [-t ] [-L] [-w] [-c] [-d] [-h]\n \ + [-t ] [-L] [-w] [-c] [-d] [-h]\n\ ", name); diff --git a/modules/imgproc/include/visp3/imgproc/vpCircleHoughTransform.h b/modules/imgproc/include/visp3/imgproc/vpCircleHoughTransform.h index b18a82b276..f02bddb803 100644 --- a/modules/imgproc/include/visp3/imgproc/vpCircleHoughTransform.h +++ b/modules/imgproc/include/visp3/imgproc/vpCircleHoughTransform.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -27,8 +26,7 @@ * * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - * - *****************************************************************************/ + */ #ifndef _vpCircleHoughTransform_h_ #define _vpCircleHoughTransform_h_ @@ -53,6 +51,7 @@ // 3rd parties inclue #ifdef VISP_HAVE_NLOHMANN_JSON #include +//! json namespace shortcut using json = nlohmann::json; #endif @@ -105,6 +104,9 @@ class VISP_EXPORT vpCircleHoughTransform friend class vpCircleHoughTransform; public: + /** + * \brief Construct a new vpCircleHoughTransformParameters object with default parameters. + */ vpCircleHoughTransformParameters() : m_gaussianKernelSize(5) , m_gaussianStdev(1.f) @@ -186,6 +188,9 @@ class VISP_EXPORT vpCircleHoughTransform } + /** + * Create a string with all the Hough transform parameters. + */ std::string toString() const { std::string txt("Hough Circle Transform Configuration:\n"); @@ -340,13 +345,13 @@ class VISP_EXPORT vpCircleHoughTransform {"circlePerfectnessThreshold", params.m_circlePerfectness}, {"centerMinDistance", params.m_centerMinDist}, {"mergingRadiusDiffThresh", params.m_mergingRadiusDiffThresh} }; - } + } #endif - }; +}; - /** - * \brief Construct a new vpCircleHoughTransform object with default parameters. - */ +/** + * \brief Construct a new vpCircleHoughTransform object with default parameters. + */ vpCircleHoughTransform(); /** @@ -719,9 +724,14 @@ class VISP_EXPORT vpCircleHoughTransform return m_algoParams.m_maxRadius; } - // // Debug methods + /*! + * Create a string with all Hough transform parameters. + */ std::string toString() const; + /*! + * Create a ostream with all Hough transform parameters. + */ friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpCircleHoughTransform &detector); private: diff --git a/modules/imgproc/include/visp3/imgproc/vpContours.h b/modules/imgproc/include/visp3/imgproc/vpContours.h index c2dad0c84a..b51e96d627 100644 --- a/modules/imgproc/include/visp3/imgproc/vpContours.h +++ b/modules/imgproc/include/visp3/imgproc/vpContours.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -31,11 +30,7 @@ * Description: * Basic contours extraction based on the orignal work of * Sina Samangooei (ss@ecs.soton.ac.uk). - * - * Authors: - * Souriya Trinh - * -*****************************************************************************/ + */ /** * Copyright (c) 2011, The University of Southampton and the individual * contributors. All rights reserved. @@ -79,26 +74,42 @@ #include #include + namespace { -typedef enum { - NORTH, - NORTH_EAST, - EAST, - SOUTH_EAST, - SOUTH, - SOUTH_WEST, - WEST, - NORTH_WEST, - LAST_DIRECTION +/*! + * Possible directions to find a contour. + */ +typedef enum +{ + NORTH, //!< North direction + NORTH_EAST, //!< North-East direction + EAST, //!< East direction + SOUTH_EAST, //!< South-East direction + SOUTH, //!< South direction + SOUTH_WEST, //!< South-West direction + WEST, //!< West direction + NORTH_WEST, //!< North-West direction + LAST_DIRECTION //!< Number of possible directions } vpDirectionType; -struct vpDirection { +/*! + * Direction object. + */ +struct vpDirection +{ + //!< Direction vpDirectionType m_direction; + //!< Pixel increment along x to reach a given direction int m_dirx[8]; + + //!< Pixel increment along y to reach a given direction int m_diry[8]; + /*! + * Default constructor. + */ vpDirection() { m_dirx[0] = 0; @@ -120,6 +131,10 @@ struct vpDirection { m_diry[7] = -1; } + /*! + * Turn clockwise to find the next pixel along the contour. + * @return Direction to take. + */ vpDirection clockwise() { vpDirection direction; @@ -129,6 +144,10 @@ struct vpDirection { return direction; } + /*! + * Turn counter clockwise to find the next pixel along the contour. + * @return Direction to take. + */ vpDirection counterClockwise() { vpDirection direction; @@ -139,6 +158,12 @@ struct vpDirection { return direction; } + /*! + * Get the next point coordinate along the contour. + * @param I Image to process. + * @param point Current point coordinate. + * @return Next point coordinate along the contour. + */ vpImagePoint active(const vpImage &I, const vpImagePoint &point) { int yy = (int)(point.get_i() + m_diry[(int)m_direction]); @@ -156,28 +181,53 @@ struct vpDirection { namespace vp { -typedef enum { +/*! + * Type of contour. + */ +typedef enum +{ CONTOUR_OUTER, /*!< Outer contour. */ CONTOUR_HOLE /*!< Hole contour. */ } vpContourType; -typedef enum { +/*! + * Type of contour retrieval. + */ +typedef enum +{ CONTOUR_RETR_TREE, /*!< Retrieve all the contours with the hierarchy stored in a tree. */ CONTOUR_RETR_LIST, /*!< Retrieve all the contours without any hierarchy. */ CONTOUR_RETR_EXTERNAL /*!< Retrieve only external contours. */ } vpContourRetrievalType; -struct vpContour { +/*! + * Structure associated to a contour. + */ +struct vpContour +{ + //! Children contour std::vector m_children; + //! Contour type vpContourType m_contourType; + //! Parent contour vpContour *m_parent; + //! Vector of points belonging to the contour std::vector m_points; - vpContour() : m_children(), m_contourType(vp::CONTOUR_HOLE), m_parent(NULL), m_points() {} + /*! + * Default constructor. + */ + vpContour() : m_children(), m_contourType(vp::CONTOUR_HOLE), m_parent(NULL), m_points() { } - vpContour(const vpContourType &type) : m_children(), m_contourType(type), m_parent(NULL), m_points() {} + /*! + * Constructor of a given contour type. + */ + vpContour(const vpContourType &type) : m_children(), m_contourType(type), m_parent(NULL), m_points() { } + /*! + * Copy constructor. + */ vpContour(const vpContour &contour) : m_children(), m_contourType(contour.m_contourType), m_parent(NULL), m_points(contour.m_points) { @@ -191,6 +241,9 @@ struct vpContour { } } + /*! + * Destructor. + */ virtual ~vpContour() { for (std::vector::iterator it = m_children.begin(); it != m_children.end(); ++it) { @@ -202,6 +255,9 @@ struct vpContour { } } + /*! + * Copy operator. + */ vpContour &operator=(const vpContour &other) { m_contourType = other.m_contourType; @@ -215,7 +271,8 @@ struct vpContour { *it = NULL; } } - } else { + } + else { // Make the current contour the root contour // to avoid problem when deleting m_parent = NULL; @@ -231,6 +288,9 @@ struct vpContour { return *this; } + /*! + * Set parent contour. + */ void setParent(vpContour *parent) { m_parent = parent; @@ -241,11 +301,41 @@ struct vpContour { } }; +/*! + * \ingroup group_imgproc_contours + * + * Draw the input contours on the binary image. + * + * \param I : Grayscale image where we want to draw the input contours. + * \param contours : Detected contours. + * \param grayValue : Drawing grayscale color. + */ VISP_EXPORT void drawContours(vpImage &I, const std::vector > &contours, unsigned char grayValue = 255); + +/*! + * \ingroup group_imgproc_contours + * + * Draw the input contours on the color image. + * + * \param I : Color image where we want to draw the input contours. + * \param contours : Detected contours. + * \param color : Drawing color. + */ VISP_EXPORT void drawContours(vpImage &I, const std::vector > &contours, const vpColor &color); +/*! + * \ingroup group_imgproc_contours + * + * Extract contours from a binary image. + * + * \param I_original : Input binary image (0 means background, 1 means + * foreground, other values are not allowed). + * \param contours : Detected contours. + * \param contourPts : List of contours, each contour contains a list of contour points. + * \param retrievalMode : Contour retrieval mode. + */ VISP_EXPORT void findContours(const vpImage &I_original, vpContour &contours, std::vector > &contourPts, const vpContourRetrievalType &retrievalMode = vp::CONTOUR_RETR_TREE); diff --git a/modules/imgproc/include/visp3/imgproc/vpImgproc.h b/modules/imgproc/include/visp3/imgproc/vpImgproc.h index 891dbc6fa4..303ef0f499 100644 --- a/modules/imgproc/include/visp3/imgproc/vpImgproc.h +++ b/modules/imgproc/include/visp3/imgproc/vpImgproc.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Static functions for basic image processing functions. - * - * Authors: - * Souriya Trinh - * -*****************************************************************************/ + */ /*! \file vpImgproc.h @@ -53,9 +48,22 @@ namespace vp { -enum RETINEX_LEVEL { RETINEX_UNIFORM = 0, RETINEX_LOW = 1, RETINEX_HIGH = 2 }; +/*! + * Retinex level that allows to specifies distribution + * of the Gaussian blurring kernel sizes for scale division values > 2. + */ +enum RETINEX_LEVEL +{ + RETINEX_UNIFORM = 0, //!< Tends to treat all image intensities similarly. + RETINEX_LOW = 1, //!< Enhances dark regions of the image. + RETINEX_HIGH = 2 //!< Enhances the bright regions of the image +}; -typedef enum { +/*! + * Automatic thresholding method. + */ +typedef enum +{ AUTO_THRESHOLD_HUANG, /*!< Huang L.-K. and Wang M.-J.J. (1995) "Image Thresholding by Minimizing the Measures of Fuzziness" Pattern Recognition, 28(1): 41-51 @@ -83,37 +91,320 @@ typedef enum { */ } vpAutoThresholdMethod; +/*! + * \ingroup group_imgproc_brightness + * + * Adjust the brightness of a grayscale image such as the new intensity is + * alpha x old_intensity + beta. + * + * \param I : The grayscale image to adjust the brightness. + * \param alpha : Multiplication coefficient. + * \param beta : Constant value added to the old intensity. + */ VISP_EXPORT void adjust(vpImage &I, double alpha, double beta); + +/*! + * \ingroup group_imgproc_brightness + * + * Adjust the brightness of a grayscale image such as the new intensity is + * alpha x old_intensity + beta. + * + * \param I1 : The original grayscale image. + * \param I2 : The grayscale image after adjusting pixel intensities. + * \param alpha : Multiplication coefficient. + * \param beta : Constant value added to the old intensity. + */ VISP_EXPORT void adjust(const vpImage &I1, vpImage &I2, double alpha, double beta); + +/*! + * \ingroup group_imgproc_brightness + * + * Adjust the brightness of a color image such as the new intensity is alpha x + * old_intensity + beta. + * + * \param I : The color image to adjust the brightness. + * \param alpha : Multiplication coefficient. + * \param beta : Constant value added to the old intensity. + */ VISP_EXPORT void adjust(vpImage &I, const double alpha, double beta); + +/*! + * \ingroup group_imgproc_brightness + * + * Adjust the brightness of a color image such as the new intensity is alpha x + * old_intensity + beta. + * + * \param I1 : The original color image. + * \param I2 : The color image after adjusting pixel intensities. + * \param alpha : Multiplication coefficient. + * \param beta : Constant value added to the old intensity. + */ VISP_EXPORT void adjust(const vpImage &I1, vpImage &I2, double alpha, double beta); +/*! + * \ingroup group_imgproc_brightness + * + * Adjust the contrast of a grayscale image locally using the Contrast Limited + * Adaptive Histogram Equalization method. The limit parameter allows to + * limit the slope of the transformation function to prevent the + * over amplification of noise. This method is a transcription of the CLAHE + * ImageJ plugin code by Stephan Saalfeld. + * + * \param I1 : The first grayscale image. + * \param I2 : The second grayscale image after application of the CLAHE + * method. + * \param blockRadius : The size (2*blockRadius+1) of the local region + * around a pixel for which the histogram is equalized. This size should be + * larger than the size of features to be preserved. + * \param bins : The number + * of histogram bins used for histogram equalization (between 1 and 256). The + * number of histogram bins should be smaller than the number of pixels in a + * block. + * \param slope : Limits the contrast stretch in the intensity transfer + * function. Very large values will let the histogram equalization do whatever + * it wants to do, that is result in maximal local contrast. The value 1 will + * result in the original image. + * \param fast : Use the fast but less accurate + * version of the filter. The fast version does not evaluate the intensity + * transfer function for each pixel independently but for a grid of adjacent + * boxes of the given block size only and interpolates for locations in + * between. + */ VISP_EXPORT void clahe(const vpImage &I1, vpImage &I2, int blockRadius = 150, int bins = 256, float slope = 3.0f, bool fast = true); + +/*! + * \ingroup group_imgproc_brightness + * + * Adjust the contrast of a color image locally using the Contrast Limited + * Adaptive Histogram Equalization method. The limit parameter allows to + * limit the slope of the transformation function to prevent the + * over amplification of noise. This method is a transcription of the CLAHE + * ImageJ plugin code by Stephan Saalfeld. + * + * \param I1 : The first color image. + * \param I2 : The second color image after application of the CLAHE method. + * \param blockRadius : The size (2*blockRadius+1) of the local region around a + * pixel for which the histogram is equalized. This size should be larger than + * the size of features to be preserved. + * \param bins : The number of histogram + * bins used for histogram equalization (between 1 and 256). The number of + * histogram bins should be smaller than the number of pixels in a block. + * \param slope : Limits the contrast stretch in the intensity transfer + * function. Very large values will let the histogram equalization do whatever + * it wants to do, that is result in maximal local contrast. The value 1 will + * result in the original image. + * \param fast : Use the fast but less accurate + * version of the filter. The fast version does not evaluate the intensity + * transfer function for each pixel independently but for a grid of adjacent + * boxes of the given block size only and interpolates for locations in + * between. +*/ VISP_EXPORT void clahe(const vpImage &I1, vpImage &I2, int blockRadius = 150, int bins = 256, float slope = 3.0f, bool fast = true); +/*! + * \ingroup group_imgproc_histogram + * + * Adjust the contrast of a grayscale image by performing an histogram + * equalization. The intensity distribution is redistributed over the full [0 - + * 255] range such as the cumulative histogram distribution becomes linear. + * + * \param I : The grayscale image to apply histogram equalization. +*/ VISP_EXPORT void equalizeHistogram(vpImage &I); + +/*! + * \ingroup group_imgproc_histogram + * + * Adjust the contrast of a grayscale image by performing an histogram + * equalization. The intensity distribution is redistributed over the full [0 - + * 255] range such as the cumulative histogram distribution becomes linear. + * + * \param I1 : The first grayscale image. + * \param I2 : The second grayscale image after histogram equalization. + */ VISP_EXPORT void equalizeHistogram(const vpImage &I1, vpImage &I2); + +/*! + * \ingroup group_imgproc_histogram + * + * Adjust the contrast of a color image by performing an histogram + * equalization. The intensity distribution is redistributed over the full [0 - + * 255] range such as the cumulative histogram distribution becomes linear. The + * alpha channel is ignored / copied from the source alpha channel. + * + * \param I : The color image to apply histogram equalization. + * \param useHSV : If true, the histogram equalization is performed on the + * value channel (in HSV space), otherwise the histogram equalization is + * performed independently on the RGB channels. + */ VISP_EXPORT void equalizeHistogram(vpImage &I, bool useHSV = false); + +/*! + * \ingroup group_imgproc_histogram + * + * Adjust the contrast of a color image by performing an histogram + * equalization. The intensity distribution is redistributed over the full [0 - + * 255] range such as the cumulative histogram distribution becomes linear. The + * alpha channel is ignored / copied from the source alpha channel. + * + * \param I1 : The first color image. + * \param I2 : The second color image after histogram equalization. + * \param useHSV : If true, the histogram equalization is performed on the + * value channel (in HSV space), otherwise the histogram equalization is + * performed independently on the RGB channels. + */ VISP_EXPORT void equalizeHistogram(const vpImage &I1, vpImage &I2, bool useHSV = false); +/*! + * \ingroup group_imgproc_gamma + * + * Perform a gamma correction on a grayscale image. + * + * \param I : The grayscale image to apply gamma correction. + * \param gamma : Gamma value. + */ VISP_EXPORT void gammaCorrection(vpImage &I, double gamma); + +/*! + * \ingroup group_imgproc_gamma + * + * Perform a gamma correction on a grayscale image. + * + * \param I1 : The first grayscale image. + * \param I2 : The second grayscale image after gamma correction. + * \param gamma : Gamma value. + */ VISP_EXPORT void gammaCorrection(const vpImage &I1, vpImage &I2, double gamma); + +/*! + * \ingroup group_imgproc_gamma + * + * Perform a gamma correction on a color image. + * + * \param I : The color image to apply gamma correction. + * \param gamma : Gamma value. + */ VISP_EXPORT void gammaCorrection(vpImage &I, double gamma); + +/*! + * \ingroup group_imgproc_gamma + * + * Perform a gamma correction on a color image. + * + * \param I1 : The first color image. + * \param I2 : The second color image after gamma correction. + * \param gamma : Gamma value. + */ VISP_EXPORT void gammaCorrection(const vpImage &I1, vpImage &I2, double gamma); +/*! + * \ingroup group_imgproc_retinex + * + * Apply the Retinex algorithm (the input image is modified). + * \param I : The color image after application of the Retinex technique. + * \param scale : Specifies the depth of the retinex effect. Minimum value is + * 16, a value providing gross, unrefined filtering. Maximum value is 250. + * Optimal and default value is 240. + * \param scaleDiv : Specifies the number of + * iterations of the multi scale filter. Values larger than 2 exploit the + * "multiscale" nature of the algorithm. + * \param level : Specifies distribution + * of the Gaussian blurring kernel sizes for Scale division values > 2: + * - 0, tends to treat all image intensities similarly, + * - 1, enhances dark regions of the image, + * - 2, enhances the bright regions of the image. + * \param dynamic : Adjusts the color of the result. Large values produce less + * saturated images. + * \param kernelSize : Kernel size for the gaussian blur + * operation. If -1, the kernel size is calculated from the image size. + */ VISP_EXPORT void retinex(vpImage &I, int scale = 240, int scaleDiv = 3, int level = RETINEX_UNIFORM, double dynamic = 1.2, int kernelSize = -1); + +/*! + * \ingroup group_imgproc_retinex + * + * Apply the Retinex algorithm. + * \param I1 : The input color image. + * \param I2 : The output color image after application of the Retinex technique. + * \param scale : Specifies the depth of the retinex effect. Minimum + * value is 16, a value providing gross, unrefined filtering. Maximum value is + * 250. Optimal and default value is 240. + * \param scaleDiv : Specifies the + * number of iterations of the multiscale filter. Values larger than 2 exploit + * the "multiscale" nature of the algorithm. + * \param level : Specifies distribution of the Gaussian blurring kernel sizes + * for Scale division values > 2: + * - 0, tends to treat all image intensities similarly, + * - 1, enhances dark regions of the image, + * - 2, enhances the bright regions of the image. + * \param dynamic : Adjusts the color of the result. Large values produce less + * saturated images. + * \param kernelSize : Kernel size for the gaussian blur + * operation. If -1, the kernel size is calculated from the image size. + */ VISP_EXPORT void retinex(const vpImage &I1, vpImage &I2, int scale = 240, int scaleDiv = 3, int level = RETINEX_UNIFORM, double dynamic = 1.2, int kernelSize = -1); +/*! + * \ingroup group_imgproc_contrast + * + * Stretch the contrast of a grayscale image. + * + * \param I : The grayscale image to stretch the contrast. +*/ VISP_EXPORT void stretchContrast(vpImage &I); + +/*! + * \ingroup group_imgproc_contrast + * + * Stretch the contrast of a grayscale image. + * + * \param I1 : The first input grayscale image. + * \param I2 : The second output grayscale image. + */ VISP_EXPORT void stretchContrast(const vpImage &I1, vpImage &I2); + +/*! + * \ingroup group_imgproc_contrast + * + * Stretch the contrast of a color image. + * + * \param I : The color image to stretch the contrast. + */ VISP_EXPORT void stretchContrast(vpImage &I); + +/*! + * \ingroup group_imgproc_contrast + * + * Stretch the contrast of a color image. + * + * \param I1 : The first input color image. + * \param I2 : The second output color image. + */ VISP_EXPORT void stretchContrast(const vpImage &I1, vpImage &I2); +/*! + * \ingroup group_imgproc_contrast + * + * Stretch the contrast of a color image in the HSV color space. + * The saturation and value components are stretch so the hue is preserved. + * + * \param I : The color image to stretch the contrast in the HSV color space. + */ VISP_EXPORT void stretchContrastHSV(vpImage &I); + +/*! + * \ingroup group_imgproc_contrast + * + * Stretch the contrast of a color image in the HSV color space. + * The saturation and value components are stretch so the hue is preserved. + * + * \param I1 : The first input color image. + * \param I2 : The second output color image. + */ VISP_EXPORT void stretchContrastHSV(const vpImage &I1, vpImage &I2); #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) @@ -130,16 +421,73 @@ vp_deprecated VISP_EXPORT void unsharpMask(const vpImage &I, vpImage &I, float sigma, double weight = 0.6); + +/*! + * \ingroup group_imgproc_sharpening + * + * Sharpen a grayscale image using the unsharp mask technique. + * + * \param I : The input grayscale image. + * \param Ires : The output grayscale image. + * \param sigma : Standard deviation for Gaussian kernel. + * \param weight : Weight (between [0 - 1[) for the sharpening process. + */ VISP_EXPORT void unsharpMask(const vpImage &I, vpImage &Ires, float sigma, double weight = 0.6); + +/*! + * \ingroup group_imgproc_sharpening + * + * Sharpen a color image using the unsharp mask technique. + * + * \param I : The color image to sharpen. + * \param sigma : Standard deviation for Gaussian kernel. + * \param weight : Weight (between [0 - 1[) for the sharpening process. + */ VISP_EXPORT void unsharpMask(vpImage &I, float sigma, double weight = 0.6); + +/*! + * \ingroup group_imgproc_sharpening + * + * Sharpen a color image using the unsharp mask technique. + * + * \param I : The input color image. + * \param Ires : The output color image. + * \param sigma : Standard deviation for Gaussian kernel. + * \param weight : Weight (between [0 - 1[) for the sharpening process. + */ VISP_EXPORT void unsharpMask(const vpImage &I, vpImage &Ires, float sigma, double weight = 0.6); -VISP_EXPORT void -connectedComponents(const vpImage &I, vpImage &labels, int &nbComponents, - const vpImageMorphology::vpConnexityType &connexity = vpImageMorphology::CONNEXITY_4); +/*! + * \ingroup group_imgproc_connected_components + * + * Perform connected components detection. + * + * \param I : Input image (0 means background). + * \param labels : Label image that contain for each position the component label. + * \param nbComponents : Number of connected components. + * \param connexity : Type of connexity. + */ +VISP_EXPORT void connectedComponents(const vpImage &I, vpImage &labels, int &nbComponents, + const vpImageMorphology::vpConnexityType &connexity = vpImageMorphology::CONNEXITY_4); +/*! + * \ingroup group_imgproc_morph + * + * Fill the holes in a binary image. + * + * \param I : Input binary image (0 means background, 255 means foreground). + */ VISP_EXPORT void fillHoles(vpImage &I #if USE_OLD_FILL_HOLE , @@ -147,14 +495,53 @@ VISP_EXPORT void fillHoles(vpImage &I #endif ); +/*! + * \ingroup group_imgproc_connected_components + * + * Perform the flood fill algorithm. + * + * \param I : Input image to flood fill. + * \param seedPoint : Seed position in the image. + * \param oldValue : Old value to replace. + * \param newValue : New value to flood fill. + * \param connexity : Type of connexity. + */ VISP_EXPORT void floodFill(vpImage &I, const vpImagePoint &seedPoint, const unsigned char oldValue, const unsigned char newValue, const vpImageMorphology::vpConnexityType &connexity = vpImageMorphology::CONNEXITY_4); +/*! + * \ingroup group_imgproc_morph + * + * Perform morphological reconstruction of the image \a marker under the image + * \a mask. Definition from Gleb V. Tcheslavsk: > The morphological + * reconstruction by dilation of a grayscale image \f$ g \f$ by a grayscale + * marker image \f$ f \f$ > is defined as the geodesic dilation of \f$ f \f$ + * with respect to \f$ g \f$ repeated (iterated) until stability is reached: + * \f[ + * R_{g}^{D} \left ( f \right ) = D_{g}^{\left ( k \right )} \left ( f \right + * ) \f] with \f$ k \f$ such that: \f$ D_{g}^{\left ( k \right )} \left ( f + * \right ) = D_{g}^{\left ( k+1 \right )} \left ( f \right ) \f$ + * + * \param marker : Grayscale image marker. + * \param mask : Grayscale image mask. + * \param h_kp1 : Image morphologically reconstructed. + * \param connexity : Type of connexity. + */ VISP_EXPORT void reconstruct(const vpImage &marker, const vpImage &mask, - vpImage &I, + vpImage &h_kp1 /*alias I */, const vpImageMorphology::vpConnexityType &connexity = vpImageMorphology::CONNEXITY_4); +/*! + * \ingroup group_imgproc_threshold + * + * Automatic thresholding. + * + * \param I : Input grayscale image. + * \param method : Automatic thresholding method. + * \param backgroundValue : Value to set to the background. + * \param foregroundValue : Value to set to the foreground. + */ VISP_EXPORT unsigned char autoThreshold(vpImage &I, const vp::vpAutoThresholdMethod &method, const unsigned char backgroundValue = 0, const unsigned char foregroundValue = 255); diff --git a/modules/imgproc/src/vpCLAHE.cpp b/modules/imgproc/src/vpCLAHE.cpp index 5137565981..2338ebeb4f 100644 --- a/modules/imgproc/src/vpCLAHE.cpp +++ b/modules/imgproc/src/vpCLAHE.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * CLAHE (Contrast Limited Adaptive Histogram Equalization) algorithm. - * - * Authors: - * Souriya Trinh - * -*****************************************************************************/ + */ /** * License: GPL * @@ -198,35 +193,6 @@ float transferValue(int v, const std::vector &hist, std::vector &clipp namespace vp { -/*! - \ingroup group_imgproc_brightness - - Adjust the contrast of a grayscale image locally using the Contrast Limited - Adaptative Histogram Equalization method. The limit parameter allows to - limit the slope of the transformation function to prevent the - over amplification of noise. This method is a transcription of the CLAHE - ImageJ plugin code by Stephan Saalfeld. - - \param I1 : The first grayscale image. - \param I2 : The second grayscale image after application of the CLAHE - method. - \param blockRadius : The size (2*blockRadius+1) of the local region - around a pixel for which the histogram is equalized. This size should be - larger than the size of features to be preserved. - \param bins : The number - of histogram bins used for histogram equalization (between 1 and 256). The - number of histogram bins should be smaller than the number of pixels in a - block. - \param slope : Limits the contrast stretch in the intensity transfer - function. Very large values will let the histogram equalization do whatever - it wants to do, that is result in maximal local contrast. The value 1 will - result in the original image. - \param fast : Use the fast but less accurate - version of the filter. The fast version does not evaluate the intensity - transfer function for each pixel independently but for a grid of adjacent - boxes of the given block size only and interpolates for locations in - between. -*/ void clahe(const vpImage &I1, vpImage &I2, int blockRadius, int bins, float slope, bool fast) { if (blockRadius < 0) { @@ -471,31 +437,6 @@ void clahe(const vpImage &I1, vpImage &I2, int blo } } -/*! - \ingroup group_imgproc_brightness - - Adjust the contrast of a color image locally using the Contrast Limited - Adaptative Histogram Equalization method. The limit parameter allows to - limit the slope of the transformation function to prevent the - overamplification of noise. This method is a transcription of the CLAHE - ImageJ plugin code by Stephan Saalfeld. - - \param I1 : The first color image. - \param I2 : The second color image after application of the CLAHE method. - \param blockRadius : The size (2*blockRadius+1) of the local region around a - pixel for which the histogram is equalized. This size should be larger than - the size of features to be preserved. \param bins : The number of histogram - bins used for histogram equalization (between 1 and 256). The number of - histogram bins should be smaller than the number of pixels in a block. - \param slope : Limits the contrast stretch in the intensity transfer - function. Very large values will let the histogram equalization do whatever - it wants to do, that is result in maximal local contrast. The value 1 will - result in the original image. \param fast : Use the fast but less accurate - version of the filter. The fast version does not evaluate the intensity - transfer function for each pixel independently but for a grid of adjacent - boxes of the given block size only and interpolates for locations in - between. -*/ void clahe(const vpImage &I1, vpImage &I2, int blockRadius, int bins, float slope, bool fast) { // Split diff --git a/modules/imgproc/src/vpCircleHoughTransform.cpp b/modules/imgproc/src/vpCircleHoughTransform.cpp index a7f1da7c21..223e36f8d5 100644 --- a/modules/imgproc/src/vpCircleHoughTransform.cpp +++ b/modules/imgproc/src/vpCircleHoughTransform.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -27,8 +26,7 @@ * * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - * - *****************************************************************************/ + */ #include #include diff --git a/modules/imgproc/src/vpConnectedComponents.cpp b/modules/imgproc/src/vpConnectedComponents.cpp index bfb62cbb35..718936c77a 100644 --- a/modules/imgproc/src/vpConnectedComponents.cpp +++ b/modules/imgproc/src/vpConnectedComponents.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Connected components. - * - * Authors: - * Souriya Trinh - * -*****************************************************************************/ + */ /*! \file vpConnectedComponents.cpp @@ -109,16 +104,6 @@ void visitNeighbors(vpImage &I_copy, std::queue &li namespace vp { -/*! - \ingroup group_imgproc_connected_components - - Perform connected components detection. - - \param I : Input image (0 means background). - \param labels : Label image that contain for each position the component - label. \param nbComponents : Number of connected components. - \param connexity : Type of connexity. -*/ void connectedComponents(const vpImage &I, vpImage &labels, int &nbComponents, const vpImageMorphology::vpConnexityType &connexity) { if (I.getSize() == 0) { diff --git a/modules/imgproc/src/vpContours.cpp b/modules/imgproc/src/vpContours.cpp index d80adb7eea..36b86af5fc 100644 --- a/modules/imgproc/src/vpContours.cpp +++ b/modules/imgproc/src/vpContours.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -31,11 +30,7 @@ * Description: * Basic contours extraction based on the original work of * Sina Samangooei (ss@ecs.soton.ac.uk). - * - * Authors: - * Souriya Trinh - * -*****************************************************************************/ + */ /** * Copyright (c) 2011, The University of Southampton and the individual * contributors. All rights reserved. @@ -248,15 +243,6 @@ void getContoursList(const vp::vpContour &root, int level, vp::vpContour &contou namespace vp { -/*! - \ingroup group_imgproc_contours - - Draw the input contours on the binary image. - - \param I : Grayscale image where we want to draw the input contours. - \param contours : Detected contours. - \param grayValue : Drawing grayscale color. -*/ void drawContours(vpImage &I, const std::vector > &contours, unsigned char grayValue) { if (I.getSize() == 0) { @@ -272,15 +258,6 @@ void drawContours(vpImage &I, const std::vector &I, const std::vector > &contours, const vpColor &color) { if (I.getSize() == 0) { @@ -296,17 +273,6 @@ void drawContours(vpImage &I, const std::vector &I_original, vpContour &contours, std::vector > &contourPts, const vpContourRetrievalType &retrievalMode) { diff --git a/modules/imgproc/src/vpFloodFill.cpp b/modules/imgproc/src/vpFloodFill.cpp index 5df3ccbc9d..cbcf1cb81e 100644 --- a/modules/imgproc/src/vpFloodFill.cpp +++ b/modules/imgproc/src/vpFloodFill.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Flood fill algorithm. - * - * Authors: - * Souriya Trinh - * -*****************************************************************************/ + */ /* * Copyright (c) 2004-2007, Lode Vandevenne * @@ -73,17 +68,6 @@ namespace vp { -/*! - \ingroup group_imgproc_connected_components - - Perform the flood fill algorithm. - - \param I : Input image to flood fill. - \param seedPoint : Seed position in the image. - \param oldValue : Old value to replace. - \param newValue : New value to flood fill. - \param connexity : Type of connexity. -*/ void floodFill(vpImage &I, const vpImagePoint &seedPoint, const unsigned char oldValue, const unsigned char newValue, const vpImageMorphology::vpConnexityType &connexity) { diff --git a/modules/imgproc/src/vpImgproc.cpp b/modules/imgproc/src/vpImgproc.cpp index 2427d1bdc9..d607689620 100644 --- a/modules/imgproc/src/vpImgproc.cpp +++ b/modules/imgproc/src/vpImgproc.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Convert image types. - * - * Authors: - * Souriya Trinh - * -*****************************************************************************/ + */ /* Autostretch HSV 0.10 --- image filter plug-in for GIMP * * Copyright (C) 1997 Scott Goehring @@ -70,16 +65,6 @@ namespace vp { -/*! - \ingroup group_imgproc_brightness - - Adjust the brightness of a grayscale image such as the new intensity is - alpha x old_intensity + beta. - - \param I : The grayscale image to adjust the brightness. - \param alpha : Multiplication coefficient. - \param beta : Constant value added to the old intensity. -*/ void adjust(vpImage &I, double alpha, double beta) { // Construct the look-up table @@ -92,17 +77,6 @@ void adjust(vpImage &I, double alpha, double beta) I.performLut(lut); } -/*! - \ingroup group_imgproc_brightness - - Adjust the brightness of a grayscale image such as the new intensity is - alpha x old_intensity + beta. - - \param I1 : The original grayscale image. - \param I2 : The grayscale image after adjusting pixel intensities. - \param alpha : Multiplication coefficient. - \param beta : Constant value added to the old intensity. -*/ void adjust(const vpImage &I1, vpImage &I2, double alpha, double beta) { // Copy I1 to I2 @@ -111,16 +85,6 @@ void adjust(const vpImage &I1, vpImage &I2, double vp::adjust(I2, alpha, beta); } -/*! - \ingroup group_imgproc_brightness - - Adjust the brightness of a color image such as the new intensity is alpha x - old_intensity + beta. - - \param I : The color image to adjust the brightness. - \param alpha : Multiplication coefficient. - \param beta : Constant value added to the old intensity. -*/ void adjust(vpImage &I, double alpha, double beta) { // Construct the look-up table @@ -136,17 +100,6 @@ void adjust(vpImage &I, double alpha, double beta) I.performLut(lut); } -/*! - \ingroup group_imgproc_brightness - - Adjust the brightness of a color image such as the new intensity is alpha x - old_intensity + beta. - - \param I1 : The original color image. - \param I2 : The color image after adjusting pixel intensities. - \param alpha : Multiplication coefficient. - \param beta : Constant value added to the old intensity. -*/ void adjust(const vpImage &I1, vpImage &I2, double alpha, double beta) { // Copy I1 to I2 @@ -155,15 +108,6 @@ void adjust(const vpImage &I1, vpImage &I2, double alpha, double vp::adjust(I2, alpha, beta); } -/*! - \ingroup group_imgproc_histogram - - Adjust the contrast of a grayscale image by performing an histogram - equalization. The intensity distribution is redistributed over the full [0 - - 255] range such as the cumulative histogram distribution becomes linear. - - \param I : The grayscale image to apply histogram equalization. -*/ void equalizeHistogram(vpImage &I) { if (I.getWidth() * I.getHeight() == 0) { @@ -216,35 +160,12 @@ void equalizeHistogram(vpImage &I) I.performLut(lut); } -/*! - \ingroup group_imgproc_histogram - - Adjust the contrast of a grayscale image by performing an histogram - equalization. The intensity distribution is redistributed over the full [0 - - 255] range such as the cumulative histogram distribution becomes linear. - - \param I1 : The first grayscale image. - \param I2 : The second grayscale image after histogram equalization. -*/ void equalizeHistogram(const vpImage &I1, vpImage &I2) { I2 = I1; vp::equalizeHistogram(I2); } -/*! - \ingroup group_imgproc_histogram - - Adjust the contrast of a color image by performing an histogram - equalization. The intensity distribution is redistributed over the full [0 - - 255] range such as the cumulative histogram distribution becomes linear. The - alpha channel is ignored / copied from the source alpha channel. - - \param I : The color image to apply histogram equalization. - \param useHSV : If true, the histogram equalization is performed on the - value channel (in HSV space), otherwise the histogram equalization is - performed independently on the RGB channels. -*/ void equalizeHistogram(vpImage &I, bool useHSV) { if (I.getWidth() * I.getHeight() == 0) { @@ -307,34 +228,12 @@ void equalizeHistogram(vpImage &I, bool useHSV) } } -/*! - \ingroup group_imgproc_histogram - - Adjust the contrast of a color image by performing an histogram - equalization. The intensity distribution is redistributed over the full [0 - - 255] range such as the cumulative histogram distribution becomes linear. The - alpha channel is ignored / copied from the source alpha channel. - - \param I1 : The first color image. - \param I2 : The second color image after histogram equalization. - \param useHSV : If true, the histogram equalization is performed on the - value channel (in HSV space), otherwise the histogram equalization is - performed independently on the RGB channels. -*/ void equalizeHistogram(const vpImage &I1, vpImage &I2, bool useHSV) { I2 = I1; vp::equalizeHistogram(I2, useHSV); } -/*! - \ingroup group_imgproc_gamma - - Perform a gamma correction on a grayscale image. - - \param I : The grayscale image to apply gamma correction. - \param gamma : Gamma value. -*/ void gammaCorrection(vpImage &I, double gamma) { double inverse_gamma = 1.0; @@ -354,29 +253,12 @@ void gammaCorrection(vpImage &I, double gamma) I.performLut(lut); } -/*! - \ingroup group_imgproc_gamma - - Perform a gamma correction on a grayscale image. - - \param I1 : The first grayscale image. - \param I2 : The second grayscale image after gamma correction. - \param gamma : Gamma value. -*/ void gammaCorrection(const vpImage &I1, vpImage &I2, double gamma) { I2 = I1; vp::gammaCorrection(I2, gamma); } -/*! - \ingroup group_imgproc_gamma - - Perform a gamma correction on a color image. - - \param I : The color image to apply gamma correction. - \param gamma : Gamma value. -*/ void gammaCorrection(vpImage &I, double gamma) { double inverse_gamma = 1.0; @@ -399,28 +281,12 @@ void gammaCorrection(vpImage &I, double gamma) I.performLut(lut); } -/*! - \ingroup group_imgproc_gamma - - Perform a gamma correction on a color image. - - \param I1 : The first color image. - \param I2 : The second color image after gamma correction. - \param gamma : Gamma value. -*/ void gammaCorrection(const vpImage &I1, vpImage &I2, double gamma) { I2 = I1; vp::gammaCorrection(I2, gamma); } -/*! - \ingroup group_imgproc_contrast - - Stretch the contrast of a grayscale image. - - \param I : The grayscale image to stretch the contrast. -*/ void stretchContrast(vpImage &I) { // Find min and max intensity values @@ -443,14 +309,6 @@ void stretchContrast(vpImage &I) I.performLut(lut); } -/*! - \ingroup group_imgproc_contrast - - Stretch the contrast of a grayscale image. - - \param I1 : The first input grayscale image. - \param I2 : The second output grayscale image. -*/ void stretchContrast(const vpImage &I1, vpImage &I2) { // Copy I1 to I2 @@ -458,13 +316,6 @@ void stretchContrast(const vpImage &I1, vpImage &I vp::stretchContrast(I2); } -/*! - \ingroup group_imgproc_contrast - - Stretch the contrast of a color image. - - \param I : The color image to stretch the contrast. -*/ void stretchContrast(vpImage &I) { // Find min and max intensity values @@ -540,14 +391,6 @@ void stretchContrast(vpImage &I) I.performLut(lut); } -/*! - \ingroup group_imgproc_contrast - - Stretch the contrast of a color image. - - \param I1 : The first input color image. - \param I2 : The second output color image. -*/ void stretchContrast(const vpImage &I1, vpImage &I2) { // Copy I1 to I2 @@ -555,14 +398,6 @@ void stretchContrast(const vpImage &I1, vpImage &I2) vp::stretchContrast(I2); } -/*! - \ingroup group_imgproc_contrast - - Stretch the contrast of a color image in the HSV color space. - The saturation and value components are stretch so the hue is preserved. - - \param I : The color image to stretch the contrast in the HSV color space. -*/ void stretchContrastHSV(vpImage &I) { unsigned int size = I.getWidth() * I.getHeight(); @@ -607,15 +442,6 @@ void stretchContrastHSV(vpImage &I) size); } -/*! - \ingroup group_imgproc_contrast - - Stretch the contrast of a color image in the HSV color space. - The saturation and value components are stretch so the hue is preserved. - - \param I1 : The first input color image. - \param I2 : The second output color image. -*/ void stretchContrastHSV(const vpImage &I1, vpImage &I2) { // Copy I1 to I2 @@ -623,15 +449,6 @@ void stretchContrastHSV(const vpImage &I1, vpImage &I2) vp::stretchContrastHSV(I2); } -/*! - \ingroup group_imgproc_sharpening - - Sharpen a grayscale image using the unsharp mask technique. - - \param I : The grayscale image to sharpen. - \param sigma : Standard deviation for Gaussian kernel. - \param weight : Weight (between [0 - 1[) for the sharpening process. - */ void unsharpMask(vpImage &I, float sigma, double weight) { if (weight < 1.0 && weight >= 0.0) { @@ -648,32 +465,13 @@ void unsharpMask(vpImage &I, float sigma, double weight) } } -/*! - \ingroup group_imgproc_sharpening - - Sharpen a grayscale image using the unsharp mask technique. - - \param I1 : The first input grayscale image. - \param I2 : The second output grayscale image. - \param sigma : Standard deviation for Gaussian kernel. - \param weight : Weight (between [0 - 1[) for the sharpening process. -*/ -void unsharpMask(const vpImage &I1, vpImage &I2, float sigma, double weight) +void unsharpMask(const vpImage &I, vpImage &Ires, float sigma, double weight) { - // Copy I1 to I2 - I2 = I1; - vp::unsharpMask(I2, sigma, weight); + // Copy I to Ires + Ires = I; + vp::unsharpMask(Ires, sigma, weight); } -/*! - \ingroup group_imgproc_sharpening - - Sharpen a color image using the unsharp mask technique. - - \param I : The color image to sharpen. - \param sigma : Standard deviation for Gaussian kernel. - \param weight : Weight (between [0 - 1[) for the sharpening process. - */ void unsharpMask(vpImage &I, float sigma, double weight) { if (weight < 1.0 && weight >= 0.0) { @@ -695,21 +493,11 @@ void unsharpMask(vpImage &I, float sigma, double weight) } } -/*! - \ingroup group_imgproc_sharpening - - Sharpen a color image using the unsharp mask technique. - - \param I1 : The first input color image. - \param I2 : The second output color image. - \param sigma : Standard deviation for Gaussian kernel. - \param weight : Weight (between [0 - 1[) for the sharpening process. -*/ -void unsharpMask(const vpImage &I1, vpImage &I2, float sigma, double weight) +void unsharpMask(const vpImage &I, vpImage &Ires, float sigma, double weight) { - // Copy I1 to I2 - I2 = I1; - vp::unsharpMask(I2, sigma, weight); + // Copy I to Ires + Ires = I; + vp::unsharpMask(Ires, sigma, weight); } #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS diff --git a/modules/imgproc/src/vpMorph.cpp b/modules/imgproc/src/vpMorph.cpp index 658d532708..8faa2fa136 100644 --- a/modules/imgproc/src/vpMorph.cpp +++ b/modules/imgproc/src/vpMorph.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: * Additional image morphology functions. - * -*****************************************************************************/ + */ /*! \file vpMorph.cpp @@ -43,13 +41,7 @@ namespace vp { -/*! - \ingroup group_imgproc_morph - - Fill the holes in a binary image. - \param I : Input binary image (0 means background, 255 means foreground). -*/ void fillHoles(vpImage &I #if USE_OLD_FILL_HOLE , @@ -124,26 +116,8 @@ void fillHoles(vpImage &I #endif } -/*! - \ingroup group_imgproc_morph - - Perform morphological reconstruction of the image \a marker under the image - \a mask. Definition from Gleb V. Tcheslavsk: > The morphological - reconstruction by dilation of a grayscale image \f$ g \f$ by a grayscale - marker image \f$ f \f$ > is defined as the geodesic dilation of \f$ f \f$ - with respect to \f$ g \f$ repeated (iterated) until stability is reached: - \f[ - R_{g}^{D} \left ( f \right ) = D_{g}^{\left ( k \right )} \left ( f \right - ) \f] with \f$ k \f$ such that: \f$ D_{g}^{\left ( k \right )} \left ( f - \right ) = D_{g}^{\left ( k+1 \right )} \left ( f \right ) \f$ - - \param marker : Grayscale image marker. - \param mask : Grayscale image mask. - \param h_kp1 : Image morphologically reconstructed. - \param connexity : Type of connexity. -*/ void reconstruct(const vpImage &marker, const vpImage &mask, - vpImage &h_kp1 /*alias I */, const vpImageMorphology::vpConnexityType &connexity) + vpImage &h_kp1 /*alias I */, const vpImageMorphology::vpConnexityType &connexity) { if (marker.getHeight() != mask.getHeight() || marker.getWidth() != mask.getWidth()) { std::cerr << "marker.getHeight() != mask.getHeight() || " diff --git a/modules/imgproc/src/vpRetinex.cpp b/modules/imgproc/src/vpRetinex.cpp index 522551278a..4ef77185dc 100644 --- a/modules/imgproc/src/vpRetinex.cpp +++ b/modules/imgproc/src/vpRetinex.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Convert image types. - * - * Authors: - * Souriya Trinh - * -*****************************************************************************/ + */ /* Retinex_.java Using ImageJ Gaussian Filter * Retinex filter algorithm based on the plugin for GIMP. * @@ -72,7 +67,7 @@ * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License @@ -246,27 +241,6 @@ void MSRCR(vpImage &I, int _scale, int scaleDiv, int level, double dynam namespace vp { -/*! - \ingroup group_imgproc_retinex - - Apply the Retinex algorithm (the input image is modified). - \param I : The color image after application of the Retinex technique. - \param scale : Specifies the depth of the retinex effect. Minimum value is - 16, a value providing gross, unrefined filtering. Maximum value is 250. - Optimal and default value is 240. - \param scaleDiv : Specifies the number of - iterations of the multi scale filter. Values larger than 2 exploit the - "multiscale" nature of the algorithm. - \param level : Specifies distribution - of the Gaussian blurring kernel sizes for Scale division values > 2: - - 0, tends to treat all image intensities similarly, - - 1, enhances dark regions of the image, - - 2, enhances the bright regions of the image. - \param dynamic : Adjusts the color of the result. Large values produce less - saturated images. - \param kernelSize : Kernel size for the gaussian blur - operation. If -1, the kernel size is calculated from the image size. -*/ void retinex(vpImage &I, int scale, int scaleDiv, int level, const double dynamic, int kernelSize) { // Assert scale @@ -288,26 +262,6 @@ void retinex(vpImage &I, int scale, int scaleDiv, int level, const doubl MSRCR(I, scale, scaleDiv, level, dynamic, kernelSize); } -/*! - \ingroup group_imgproc_retinex - - Apply the Retinex algorithm. - \param I1 : The input color image. - \param I2 : The output color image after application of the Retinex - technique. \param scale : Specifies the depth of the retinex effect. Minimum - value is 16, a value providing gross, unrefined filtering. Maximum value is - 250. Optimal and default value is 240. \param scaleDiv : Specifies the - number of iterations of the multiscale filter. Values larger than 2 exploit - the "multiscale" nature of the algorithm. \param level : Specifies - distribution of the Gaussian blurring kernel sizes for Scale division values - > 2: - - 0, tends to treat all image intensities similarly, - - 1, enhances dark regions of the image, - - 2, enhances the bright regions of the image. - \param dynamic : Adjusts the color of the result. Large values produce less - saturated images. \param kernelSize : Kernel size for the gaussian blur - operation. If -1, the kernel size is calculated from the image size. -*/ void retinex(const vpImage &I1, vpImage &I2, int scale, int scaleDiv, int level, double dynamic, int kernelSize) { diff --git a/modules/imgproc/src/vpThreshold.cpp b/modules/imgproc/src/vpThreshold.cpp index 1564adbd1d..c1ab6ea9f9 100644 --- a/modules/imgproc/src/vpThreshold.cpp +++ b/modules/imgproc/src/vpThreshold.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Automatic thresholding functions. - * - * Authors: - * Souriya Trinh - * -*****************************************************************************/ + */ /*! \file vpThreshold.cpp @@ -363,16 +358,6 @@ int computeThresholdTriangle(vpHistogram &hist) namespace vp { -/*! - \ingroup group_imgproc_threshold - - Automatic thresholding. - - \param I : Input grayscale image. - \param method : Automatic thresholding method. - \param backgroundValue : Value to set to the background. - \param foregroundValue : Value to set to the foreground. -*/ unsigned char autoThreshold(vpImage &I, const vpAutoThresholdMethod &method, const unsigned char backgroundValue, const unsigned char foregroundValue) { diff --git a/modules/imgproc/test/with-dataset/testAutoThreshold.cpp b/modules/imgproc/test/with-dataset/testAutoThreshold.cpp index 0bee8c5715..e25b9a2c8e 100644 --- a/modules/imgproc/test/with-dataset/testAutoThreshold.cpp +++ b/modules/imgproc/test/with-dataset/testAutoThreshold.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Test automatic thresholding. - * - * Authors: - * Souriya Trinh - * -*****************************************************************************/ + */ #include #include diff --git a/modules/imgproc/test/with-dataset/testConnectedComponents.cpp b/modules/imgproc/test/with-dataset/testConnectedComponents.cpp index 91b324ae1d..64d5aca7f8 100644 --- a/modules/imgproc/test/with-dataset/testConnectedComponents.cpp +++ b/modules/imgproc/test/with-dataset/testConnectedComponents.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 connected components. - * -*****************************************************************************/ + */ #include #include #include diff --git a/modules/imgproc/test/with-dataset/testContours.cpp b/modules/imgproc/test/with-dataset/testContours.cpp index 3c61867ee6..e094dd7dc7 100644 --- a/modules/imgproc/test/with-dataset/testContours.cpp +++ b/modules/imgproc/test/with-dataset/testContours.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 contours extraction. - * -*****************************************************************************/ + */ #include diff --git a/modules/imgproc/test/with-dataset/testFloodFill.cpp b/modules/imgproc/test/with-dataset/testFloodFill.cpp index 38479c7957..b6ecd2e7a1 100644 --- a/modules/imgproc/test/with-dataset/testFloodFill.cpp +++ b/modules/imgproc/test/with-dataset/testFloodFill.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 flood fill algorithm. - * -*****************************************************************************/ + */ #include diff --git a/modules/imgproc/test/with-dataset/testImgproc.cpp b/modules/imgproc/test/with-dataset/testImgproc.cpp index 806a81b819..6307069edb 100644 --- a/modules/imgproc/test/with-dataset/testImgproc.cpp +++ b/modules/imgproc/test/with-dataset/testImgproc.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Test imgproc functions. - * - * Authors: - * Souriya Trinh - * -*****************************************************************************/ + */ #include #include diff --git a/modules/io/include/visp3/io/vpDiskGrabber.h b/modules/io/include/visp3/io/vpDiskGrabber.h index 2925f918d8..1548e92a5f 100644 --- a/modules/io/include/visp3/io/vpDiskGrabber.h +++ b/modules/io/include/visp3/io/vpDiskGrabber.h @@ -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: * Disk framegrabber. - * -*****************************************************************************/ + */ /*! \file vpDiskGrabber.h @@ -120,41 +118,155 @@ class VISP_EXPORT vpDiskGrabber : public vpFrameGrabber std::string m_image_name; public: + /*! + * Default constructor. + */ vpDiskGrabber(); + + /*! + * Constructor that takes a generic image sequence as input. + */ explicit vpDiskGrabber(const std::string &genericName); + + /*! + * Constructor. + * + * \param dir : Location of the image sequence. + * \param basename : Base name of each image. + * \param number : Initial image number. + * \param step : Increment between two images. + * \param noz : Number of zero to code the image number. + * \param ext : Extension of the image file. + */ explicit vpDiskGrabber(const std::string &dir, const std::string &basename, long number, int step, unsigned int noz, const std::string &ext); + + /*! + * Destructor. + * In fact nothing to destroy... + */ virtual ~vpDiskGrabber(); + /*! + * Acquire an image reading the next image from the disk. + * After this call, the image number is incremented considering the step. + * + * \param I : The image read from a file. + */ void acquire(vpImage &I); + + /*! + * Acquire an image reading the next image from the disk. + * After this call, the image number is incremented considering the step. + * + * \param I : The image read from a file. + */ void acquire(vpImage &I); + + /*! + * Acquire an image reading the next image from the disk. + * After this call, the image number is incremented considering the step. + * + * \param I : The image read from a file. + */ void acquire(vpImage &I); + + /*! + * Acquire an image reading the image with number \e img_number from the disk. + * After this call, the image number is incremented considering the step. + * + * \param I : The image read from a file. + * \param image_number : The number of the desired image. + */ void acquire(vpImage &I, long image_number); + + /*! + * Acquire an image reading the image with number \e img_number from the disk. + * After this call, the image number is incremented considering the step. + * + * \param I : The image read from a file. + * \param image_number : The number of the desired image. + */ void acquire(vpImage &I, long image_number); + + /*! + * Acquire an image reading the pfm image with number \e img_number from the + * disk. After this call, the image number is incremented considering the step. + * + * \param I : The image read from a file. + * \param image_number : The number of the desired image. + */ void acquire(vpImage &I, long image_number); - void close(); + /*! + * Not useful. + * + * Does nothing. Here for compatibility issue with the vpFrameGrabber class. + */ + void close() { }; /*! - Return the current image number. - */ + * Return the current image number. + */ inline long getImageNumber() const { return m_image_number; } + /*! * Return the name of the file in which the last frame was read. */ inline std::string getImageName() const { return m_image_name; } + /*! + * Read the first image of the sequence. + * The image number is not incremented. + */ void open(vpImage &I); + + /*! + * Read the first image of the sequence. + * The image number is not incremented. + */ void open(vpImage &I); + + /*! + * Read the first image of the sequence. + * The image number is not incremented. + */ void open(vpImage &I); - void setBaseName(const std::string &name); - void setDirectory(const std::string &dir); - void setExtension(const std::string &ext); + /*! + * Set the image base name. + */ + void setBaseName(const std::string &name) { m_base_name = name; } + + /*! + * Set the main directory name (ie location of the image sequence). + */ + void setDirectory(const std::string &dir) { m_directory = dir; } + + /*! + * Set the image extension. + */ + void setExtension(const std::string &ext) { m_extension = ext; } + + /*! + * Set the image generic name like `image-%04d.png`. + */ void setGenericName(const std::string &genericName); + + /*! + * Set the number of the image to be read. + */ void setImageNumber(long number); - void setNumberOfZero(unsigned int noz); - void setStep(long step); + + /*! + * Set the step between two images. + */ + void setNumberOfZero(unsigned int noz) { m_number_of_zero = noz; } + + /*! + * Set the step between two images. + */ + void setStep(long step) { m_image_step = step; } }; #endif diff --git a/modules/io/include/visp3/io/vpImageIo.h b/modules/io/include/visp3/io/vpImageIo.h index d13858597d..bf4eb446d0 100644 --- a/modules/io/include/visp3/io/vpImageIo.h +++ b/modules/io/include/visp3/io/vpImageIo.h @@ -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: * Read/write images. - * -*****************************************************************************/ + */ /*! \file vpImageIo.h diff --git a/modules/io/include/visp3/io/vpImageQueue.h b/modules/io/include/visp3/io/vpImageQueue.h index cb2ce665b1..b300d54701 100644 --- a/modules/io/include/visp3/io/vpImageQueue.h +++ b/modules/io/include/visp3/io/vpImageQueue.h @@ -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: * Image queue for storage helper. - * -*****************************************************************************/ + */ #ifndef vpImageQueue_h #define vpImageQueue_h diff --git a/modules/io/include/visp3/io/vpImageStorageWorker.h b/modules/io/include/visp3/io/vpImageStorageWorker.h index d5005cfc87..2fefad8c32 100644 --- a/modules/io/include/visp3/io/vpImageStorageWorker.h +++ b/modules/io/include/visp3/io/vpImageStorageWorker.h @@ -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: * Image storage helper. - * -*****************************************************************************/ + */ #ifndef vpImageStorageWorker_h #define vpImageStorageWorker_h diff --git a/modules/io/include/visp3/io/vpJsonArgumentParser.h b/modules/io/include/visp3/io/vpJsonArgumentParser.h index 983f962b8e..0ce17b6536 100644 --- a/modules/io/include/visp3/io/vpJsonArgumentParser.h +++ b/modules/io/include/visp3/io/vpJsonArgumentParser.h @@ -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: * An argument parser that can both use JSON files and command line arguments as inputs. - * -*****************************************************************************/ + */ #ifndef _vpJsonArgumentParser_h_ #define _vpJsonArgumentParser_h_ diff --git a/modules/io/include/visp3/io/vpKeyboard.h b/modules/io/include/visp3/io/vpKeyboard.h index 23fee804ca..6026faeebd 100644 --- a/modules/io/include/visp3/io/vpKeyboard.h +++ b/modules/io/include/visp3/io/vpKeyboard.h @@ -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: * Keyboard management. - * -*****************************************************************************/ + */ #ifndef vpKeyboard_h #define vpKeyboard_h diff --git a/modules/io/include/visp3/io/vpParallelPort.h b/modules/io/include/visp3/io/vpParallelPort.h index f686596c0e..043aed23ab 100644 --- a/modules/io/include/visp3/io/vpParallelPort.h +++ b/modules/io/include/visp3/io/vpParallelPort.h @@ -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: * Parallel port management. - * -*****************************************************************************/ + */ #ifndef vpParallelPort_h #define vpParallelPort_h diff --git a/modules/io/include/visp3/io/vpParallelPortException.h b/modules/io/include/visp3/io/vpParallelPortException.h index 33b975a401..f7ff7fd38e 100644 --- a/modules/io/include/visp3/io/vpParallelPortException.h +++ b/modules/io/include/visp3/io/vpParallelPortException.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,62 +29,46 @@ * * Description: * Exceptions that can be emitted by the vpParallelPort class and its - *derivates. - * -*****************************************************************************/ + * derivates. + */ #ifndef _vpParallelPortException_h_ #define _vpParallelPortException_h_ -/* ------------------------------------------------------------------------- - */ -/* --- INCLUDE ------------------------------------------------------------- - */ -/* ------------------------------------------------------------------------- - */ - /*! - - \file vpParallelPortException.h - - \brief Error that can be emitted by the vpParallelPort class and its - derivates. - -*/ - -/* Classes standards. */ + * \file vpParallelPortException.h + * \brief Error that can be emitted by the vpParallelPort class and its + * derivates. + */ #include -#include /* Classe std::ostream. */ -#include /* Classe string. */ - -/* ------------------------------------------------------------------------- - */ -/* --- CLASS --------------------------------------------------------------- - */ -/* ------------------------------------------------------------------------- - */ +#include +#include /*! - \class vpParallelPortException - - \brief Error that can be emitted by the vpParallelPort class and its - derivates. + * \class vpParallelPortException + * + * \brief Error that can be emitted by the vpParallelPort class and its + * derivates. */ class VISP_EXPORT vpParallelPortException : public vpException { public: /*! - \brief Lists the possible errors than can be emitted while calling - vpParallelPort member - */ - enum error { + * \brief Lists the possible errors than can be emitted while calling + * vpParallelPort member + */ + enum error + { opening, /*!< Cannot access to the parallel port device. */ closing /*!< Cannot close the parallel port device. */ }; public: + /*! + * Constructor. + */ vpParallelPortException(int id, const char *format, ...) { this->code = id; @@ -94,7 +77,15 @@ class VISP_EXPORT vpParallelPortException : public vpException setMessage(format, args); va_end(args); } + + /*! + * Constructor. + */ vpParallelPortException(int id, const std::string &msg) : vpException(id, msg) { } + + /*! + * Constructor. + */ explicit vpParallelPortException(int id) : vpException(id) { } }; diff --git a/modules/io/include/visp3/io/vpParseArgv.h b/modules/io/include/visp3/io/vpParseArgv.h index a7c40402d9..60bf1dd872 100644 --- a/modules/io/include/visp3/io/vpParseArgv.h +++ b/modules/io/include/visp3/io/vpParseArgv.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * Declarations for Tk-related things that are visible * outside of the Tk module itself. * @@ -15,13 +14,9 @@ * This file has been modified to be used only for argv parsing without * reference to tk, tcl or X11. Base on tk.h from tk2.3 * - * Description: - * Command line argument parsing. - * - * Authors: - * Fabien Spindler (modification of the original version) - * -*****************************************************************************/ + * Modifications by Peter Neelin (November 27, 1992) + * Modifications by Fabien Spindler (June 20, 2006) + */ /*! \file vpParseArgv.h @@ -100,7 +95,7 @@ int main(int argc, const char ** argv) #include // List of allowed command line options -#define GETOPTARGS "bi:l:f:d:h" // double point mean here that the preceding option request an argument +#define GETOPTARGS "bi:l:f:d:h" // double point mean here that the preceding option request an argument // Usage : [-b] [-i ] [-l ] // [-f ] [-d ] [-s ] [-h] @@ -116,7 +111,7 @@ int main(int argc, const char ** argv) // Parse the command line to set the variables const char *optarg; - int c; + int c; while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) { switch (c) { diff --git a/modules/io/include/visp3/io/vpVideoReader.h b/modules/io/include/visp3/io/vpVideoReader.h index 3b6351caea..ba4a294df0 100644 --- a/modules/io/include/visp3/io/vpVideoReader.h +++ b/modules/io/include/visp3/io/vpVideoReader.h @@ -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: * Read videos and sequences of images . - * -*****************************************************************************/ + */ /*! \file vpVideoReader.h diff --git a/modules/io/include/visp3/io/vpVideoWriter.h b/modules/io/include/visp3/io/vpVideoWriter.h index 247e610a32..85d6cdd977 100644 --- a/modules/io/include/visp3/io/vpVideoWriter.h +++ b/modules/io/include/visp3/io/vpVideoWriter.h @@ -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: * Write videos and sequences of images. - * -*****************************************************************************/ + */ /*! \file vpVideoWriter.h diff --git a/modules/io/src/image/private/vpImageIoBackend.h b/modules/io/src/image/private/vpImageIoBackend.h index 8295742e2a..10fde6b763 100644 --- a/modules/io/src/image/private/vpImageIoBackend.h +++ b/modules/io/src/image/private/vpImageIoBackend.h @@ -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: * Backend functions implementation for image I/O operations. - * -*****************************************************************************/ + */ /*! \file vpImageIoBackend.h diff --git a/modules/io/src/image/private/vpImageIoLibjpeg.cpp b/modules/io/src/image/private/vpImageIoLibjpeg.cpp index 873d191cd6..7ebf46c94b 100644 --- a/modules/io/src/image/private/vpImageIoLibjpeg.cpp +++ b/modules/io/src/image/private/vpImageIoLibjpeg.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: * Libjpeg backend for JPEG image I/O operations. - * -*****************************************************************************/ + */ /*! \file vpImageIoLibjpeg.cpp diff --git a/modules/io/src/image/private/vpImageIoLibpng.cpp b/modules/io/src/image/private/vpImageIoLibpng.cpp index 247d295d2f..aed697c5b5 100644 --- a/modules/io/src/image/private/vpImageIoLibpng.cpp +++ b/modules/io/src/image/private/vpImageIoLibpng.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: * Libpng backend for PNG image I/O operations. - * -*****************************************************************************/ + */ /*! \file vpImageIoLibpng.cpp @@ -89,7 +87,7 @@ void writePNGLibpng(const vpImage &I, const std::string &filename throw(vpImageException(vpImageException::ioError, "PNG write error")); } - /* initialize the setjmp for returning properly after a libpng error occured + /* initialize the setjmp for returning properly after a libpng error occurred */ if (setjmp(png_jmpbuf(png_ptr))) { fclose(file); @@ -186,7 +184,7 @@ void writePNGLibpng(const vpImage &I, const std::string &filename) throw(vpImageException(vpImageException::ioError, "PNG write error")); } - /* initialize the setjmp for returning properly after a libpng error occured + /* initialize the setjmp for returning properly after a libpng error occurred */ if (setjmp(png_jmpbuf(png_ptr))) { fclose(file); @@ -311,7 +309,7 @@ void readPNGLibpng(vpImage &I, const std::string &filename) throw(vpImageException(vpImageException::ioError, "error reading png file")); } - /* initialize the setjmp for returning properly after a libpng error occured + /* initialize the setjmp for returning properly after a libpng error occurred */ if (setjmp(png_jmpbuf(png_ptr))) { fclose(file); @@ -488,7 +486,7 @@ void readPNGLibpng(vpImage &I, const std::string &filename) throw(vpImageException(vpImageException::ioError, "PNG read error")); } - /* initialize the setjmp for returning properly after a libpng error occured + /* initialize the setjmp for returning properly after a libpng error occurred */ if (setjmp(png_jmpbuf(png_ptr))) { fclose(file); diff --git a/modules/io/src/image/private/vpImageIoOpenCV.cpp b/modules/io/src/image/private/vpImageIoOpenCV.cpp index 199373c056..6e830eccb8 100644 --- a/modules/io/src/image/private/vpImageIoOpenCV.cpp +++ b/modules/io/src/image/private/vpImageIoOpenCV.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: * OpenCV backend for image I/O operations. - * -*****************************************************************************/ + */ /*! \file vpImageIoOpenCV.cpp diff --git a/modules/io/src/image/private/vpImageIoPortable.cpp b/modules/io/src/image/private/vpImageIoPortable.cpp index 21ca165bf0..489bd1a449 100644 --- a/modules/io/src/image/private/vpImageIoPortable.cpp +++ b/modules/io/src/image/private/vpImageIoPortable.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: * Backend for portable image format I/O operations. - * -*****************************************************************************/ + */ /*! \file vpImageIoPortable.cpp diff --git a/modules/io/src/image/private/vpImageIoSimd.cpp b/modules/io/src/image/private/vpImageIoSimd.cpp index c076c68c6e..7c4238008c 100644 --- a/modules/io/src/image/private/vpImageIoSimd.cpp +++ b/modules/io/src/image/private/vpImageIoSimd.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: * Simd backend for JPEG and PNG image I/O operations. - * -*****************************************************************************/ + */ /*! \file vpImageIo.cpp diff --git a/modules/io/src/image/private/vpImageIoStb.cpp b/modules/io/src/image/private/vpImageIoStb.cpp index 306b2d1085..3db5e29804 100644 --- a/modules/io/src/image/private/vpImageIoStb.cpp +++ b/modules/io/src/image/private/vpImageIoStb.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: * stb backend for JPEG and PNG image I/O operations. - * -*****************************************************************************/ + */ /*! \file vpImageIo.cpp diff --git a/modules/io/src/image/private/vpImageIoTinyEXR.cpp b/modules/io/src/image/private/vpImageIoTinyEXR.cpp index c16a2d042c..02766d6822 100644 --- a/modules/io/src/image/private/vpImageIoTinyEXR.cpp +++ b/modules/io/src/image/private/vpImageIoTinyEXR.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: * TinyEXR backend for EXR image I/O operations. - * -*****************************************************************************/ + */ /*! \file vpImageIoTinyEXR.cpp diff --git a/modules/io/src/image/vpImageIo.cpp b/modules/io/src/image/vpImageIo.cpp index 1744f6b1ba..a74a61b529 100644 --- a/modules/io/src/image/vpImageIo.cpp +++ b/modules/io/src/image/vpImageIo.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: * Read/write images. - * -*****************************************************************************/ + */ /*! \file vpImageIo.cpp diff --git a/modules/io/src/parallel-port/vpParallelPort.cpp b/modules/io/src/parallel-port/vpParallelPort.cpp index 6980d320bf..c8c0edd62e 100644 --- a/modules/io/src/parallel-port/vpParallelPort.cpp +++ b/modules/io/src/parallel-port/vpParallelPort.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: * Parallel port management. - * -*****************************************************************************/ + */ #include diff --git a/modules/io/src/tools/vpJsonArgumentParser.cpp b/modules/io/src/tools/vpJsonArgumentParser.cpp index 2af5a568e2..b392eca1f7 100644 --- a/modules/io/src/tools/vpJsonArgumentParser.cpp +++ b/modules/io/src/tools/vpJsonArgumentParser.cpp @@ -1,3 +1,32 @@ +/* + * ViSP, open source Visual Servoing Platform software. + * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * + * This software is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * See the file LICENSE.txt at the root directory of this source + * distribution for additional information about the GNU GPL. + * + * For using ViSP with software that can not be combined with the GNU + * GPL, please contact Inria about acquiring a ViSP Professional + * Edition License. + * + * See https://visp.inria.fr for more information. + * + * This software was developed at: + * Inria Rennes - Bretagne Atlantique + * Campus Universitaire de Beaulieu + * 35042 Rennes Cedex + * France + * + * If you have questions regarding the use of this file, please contact + * Inria at visp@inria.fr + * + * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE + * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. + */ #include #include #include @@ -6,7 +35,7 @@ #if defined(VISP_HAVE_NLOHMANN_JSON) -using json = nlohmann::json; +using json = nlohmann::json; //! json namespace shortcut vpJsonArgumentParser::vpJsonArgumentParser(const std::string &description, const std::string &jsonFileArgumentName, diff --git a/modules/io/src/tools/vpKeyboard.cpp b/modules/io/src/tools/vpKeyboard.cpp index 0deabc3e92..6f4a2f5f0a 100644 --- a/modules/io/src/tools/vpKeyboard.cpp +++ b/modules/io/src/tools/vpKeyboard.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: * Keyboard management. - * -*****************************************************************************/ + */ #include diff --git a/modules/io/src/video/vpDiskGrabber.cpp b/modules/io/src/video/vpDiskGrabber.cpp index 1460cc1f09..474809e416 100644 --- a/modules/io/src/video/vpDiskGrabber.cpp +++ b/modules/io/src/video/vpDiskGrabber.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,54 +29,33 @@ * * Description: * Disk framegrabber. - * -*****************************************************************************/ + */ #include -/*! - Elementary constructor. -*/ vpDiskGrabber::vpDiskGrabber() : m_image_number(0), m_image_number_next(0), m_image_step(1), m_number_of_zero(0), m_directory("/tmp"), - m_base_name("I"), m_extension("pgm"), m_use_generic_name(false), m_generic_name("empty") + m_base_name("I"), m_extension("pgm"), m_use_generic_name(false), m_generic_name("empty") { init = false; } -/*! - Constructor that takes a generic image sequence as input. -*/ + vpDiskGrabber::vpDiskGrabber(const std::string &generic_name) : m_image_number(0), m_image_number_next(0), m_image_step(1), m_number_of_zero(0), m_directory("/tmp"), - m_base_name("I"), m_extension("pgm"), m_use_generic_name(true), m_generic_name(generic_name) + m_base_name("I"), m_extension("pgm"), m_use_generic_name(true), m_generic_name(generic_name) { init = false; } -/*! - Constructor. - - \param dir : Location of the image sequence. - \param basename : Base name of each image. - \param number : Initial image number. - \param step : Increment between two images. - \param noz : Number of zero to code the image number. - \param ext : Extension of the image file. -*/ - vpDiskGrabber::vpDiskGrabber(const std::string &dir, const std::string &basename, long number, int step, unsigned int noz, const std::string &ext) : m_image_number(number), m_image_number_next(number), m_image_step(step), m_number_of_zero(noz), m_directory(dir), - m_base_name(basename), m_extension(ext), m_use_generic_name(false), m_generic_name("empty"), m_image_name() + m_base_name(basename), m_extension(ext), m_use_generic_name(false), m_generic_name("empty"), m_image_name() { init = false; } -/*! - Read the first image of the sequence. - The image number is not incremented. -*/ void vpDiskGrabber::open(vpImage &I) { long first_number = getImageNumber(); @@ -92,10 +70,6 @@ void vpDiskGrabber::open(vpImage &I) init = true; } -/*! - Read the first image of the sequence. - The image number is not incremented. -*/ void vpDiskGrabber::open(vpImage &I) { // First we save the image number, so that it can be reaffected after the @@ -112,10 +86,6 @@ void vpDiskGrabber::open(vpImage &I) init = true; } -/*! - Read the first image of the sequence. - The image number is not incremented. -*/ void vpDiskGrabber::open(vpImage &I) { // First we save the image number, so that it can be reaffected after the @@ -132,12 +102,6 @@ void vpDiskGrabber::open(vpImage &I) init = true; } -/*! - Acquire an image reading the next image from the disk. - After this call, the image number is incremented considering the step. - - \param I : The image read from a file. - */ void vpDiskGrabber::acquire(vpImage &I) { m_image_number = m_image_number_next; @@ -147,9 +111,10 @@ void vpDiskGrabber::acquire(vpImage &I) char filename[FILENAME_MAX]; snprintf(filename, FILENAME_MAX, m_generic_name.c_str(), m_image_number); ss << filename; - } else { + } + else { ss << m_directory << "/" << m_base_name << std::setfill('0') << std::setw(m_number_of_zero) << m_image_number << "." - << m_extension; + << m_extension; } m_image_number_next += m_image_step; @@ -160,12 +125,6 @@ void vpDiskGrabber::acquire(vpImage &I) height = I.getHeight(); } -/*! - Acquire an image reading the next image from the disk. - After this call, the image number is incremented considering the step. - - \param I : The image read from a file. - */ void vpDiskGrabber::acquire(vpImage &I) { m_image_number = m_image_number_next; @@ -175,9 +134,10 @@ void vpDiskGrabber::acquire(vpImage &I) char filename[FILENAME_MAX]; snprintf(filename, FILENAME_MAX, m_generic_name.c_str(), m_image_number); ss << filename; - } else { + } + else { ss << m_directory << "/" << m_base_name << std::setfill('0') << std::setw(m_number_of_zero) << m_image_number << "." - << m_extension; + << m_extension; } m_image_number_next += m_image_step; @@ -188,12 +148,6 @@ void vpDiskGrabber::acquire(vpImage &I) height = I.getHeight(); } -/*! - Acquire an image reading the next pfm image from the disk. - After this call, the image number is incremented considering the step. - - \param I : The image read from a file. - */ void vpDiskGrabber::acquire(vpImage &I) { m_image_number = m_image_number_next; @@ -202,9 +156,10 @@ void vpDiskGrabber::acquire(vpImage &I) char filename[FILENAME_MAX]; snprintf(filename, FILENAME_MAX, m_generic_name.c_str(), m_image_number); ss << filename; - } else { + } + else { ss << m_directory << "/" << m_base_name << std::setfill('0') << std::setw(m_number_of_zero) << m_image_number << "." - << m_extension; + << m_extension; } m_image_number_next += m_image_step; @@ -215,24 +170,18 @@ void vpDiskGrabber::acquire(vpImage &I) height = I.getHeight(); } -/*! - Acquire an image reading the image with number \e img_number from the disk. - After this call, the image number is incremented considering the step. - - \param I : The image read from a file. - \param img_number : The number of the desired image. - */ -void vpDiskGrabber::acquire(vpImage &I, long img_number) +void vpDiskGrabber::acquire(vpImage &I, long image_number) { - m_image_number = img_number; + m_image_number = image_number; std::stringstream ss; if (m_use_generic_name) { char filename[FILENAME_MAX]; snprintf(filename, FILENAME_MAX, m_generic_name.c_str(), m_image_number); ss << filename; - } else { + } + else { ss << m_directory << "/" << m_base_name << std::setfill('0') << std::setw(m_number_of_zero) << m_image_number << "." - << m_extension; + << m_extension; } m_image_number_next = m_image_number + m_image_step; @@ -243,24 +192,18 @@ void vpDiskGrabber::acquire(vpImage &I, long img_number) height = I.getHeight(); } -/*! - Acquire an image reading the image with number \e img_number from the disk. - After this call, the image number is incremented considering the step. - - \param I : The image read from a file. - \param img_number : The number of the desired image. - */ -void vpDiskGrabber::acquire(vpImage &I, long img_number) +void vpDiskGrabber::acquire(vpImage &I, long image_number) { - m_image_number = img_number; + m_image_number = image_number; std::stringstream ss; if (m_use_generic_name) { char filename[FILENAME_MAX]; snprintf(filename, FILENAME_MAX, m_generic_name.c_str(), m_image_number); ss << filename; - } else { + } + else { ss << m_directory << "/" << m_base_name << std::setfill('0') << std::setw(m_number_of_zero) << m_image_number << "." - << m_extension; + << m_extension; } m_image_number_next = m_image_number + m_image_step; @@ -271,14 +214,7 @@ void vpDiskGrabber::acquire(vpImage &I, long img_number) height = I.getHeight(); } -/*! - Acquire an image reading the pfm image with number \e img_number from the - disk. After this call, the image number is incremented considering the step. - - \param I : The image read from a file. - \param img_number : The number of the desired image. - */ -void vpDiskGrabber::acquire(vpImage &I, long img_number) +void vpDiskGrabber::acquire(vpImage &I, long image_number) { m_image_number = m_image_number_next; std::stringstream ss; @@ -286,9 +222,10 @@ void vpDiskGrabber::acquire(vpImage &I, long img_number) char filename[FILENAME_MAX]; snprintf(filename, FILENAME_MAX, m_generic_name.c_str(), m_image_number); ss << filename; - } else { - ss << m_directory << "/" << m_base_name << std::setfill('0') << std::setw(m_number_of_zero) << img_number << "." - << m_extension; + } + else { + ss << m_directory << "/" << m_base_name << std::setfill('0') << std::setw(m_number_of_zero) << image_number << "." + << m_extension; } m_image_number_next += m_image_step; @@ -299,56 +236,14 @@ void vpDiskGrabber::acquire(vpImage &I, long img_number) height = I.getHeight(); } -/*! - Not useful. - - Here for compatibility issue with the vpFrameGrabber class. - */ -void vpDiskGrabber::close() -{ - // Nothing do do here... -} - -/*! - Destructor - - In fact nothing to destroy... - */ -vpDiskGrabber::~vpDiskGrabber() {} - -/*! - Set the main directory name (ie location of the image sequence) -*/ -void vpDiskGrabber::setDirectory(const std::string &dir) { m_directory = dir; } +vpDiskGrabber::~vpDiskGrabber() { } -/*! - Set the image base name. -*/ -void vpDiskGrabber::setBaseName(const std::string &name) { m_base_name = name; } - -/*! - Set the image extension. - */ -void vpDiskGrabber::setExtension(const std::string &ext) { m_extension = ext; } - -/*! - Set the number of the image to be read. -*/ void vpDiskGrabber::setImageNumber(long number) { m_image_number = number; m_image_number_next = number; } -/*! - Set the step between two images. -*/ -void vpDiskGrabber::setStep(long step) { m_image_step = step; } -/*! - Set the step between two images. -*/ -void vpDiskGrabber::setNumberOfZero(unsigned int noz) { m_number_of_zero = noz; } - void vpDiskGrabber::setGenericName(const std::string &generic_name) { m_generic_name = generic_name; diff --git a/modules/io/src/video/vpVideoReader.cpp b/modules/io/src/video/vpVideoReader.cpp index 5ed8cc32cc..b687c48204 100644 --- a/modules/io/src/video/vpVideoReader.cpp +++ b/modules/io/src/video/vpVideoReader.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: * Read videos and image sequences. - * -*****************************************************************************/ + */ /*! \file vpVideoReader.cpp diff --git a/modules/io/src/video/vpVideoWriter.cpp b/modules/io/src/video/vpVideoWriter.cpp index 54ddfe0df0..189c9f356f 100644 --- a/modules/io/src/video/vpVideoWriter.cpp +++ b/modules/io/src/video/vpVideoWriter.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: * Write image sequences. - * -*****************************************************************************/ + */ /*! \file vpVideoWriter.cpp diff --git a/modules/io/test/testJsonArgumentParser.cpp b/modules/io/test/testJsonArgumentParser.cpp index b5d2762f34..be315000a0 100644 --- a/modules/io/test/testJsonArgumentParser.cpp +++ b/modules/io/test/testJsonArgumentParser.cpp @@ -44,7 +44,7 @@ #if defined(VISP_HAVE_NLOHMANN_JSON) && defined(VISP_HAVE_CATCH2) #include -using json = nlohmann::json; +using json = nlohmann::json; //! json namespace shortcut #define CATCH_CONFIG_RUNNER #include diff --git a/modules/java/generator/android-21/java/org/visp/android/JavaCamera2View.java b/modules/java/generator/android-21/java/org/visp/android/JavaCamera2View.java index 020aabac30..2360742eec 100644 --- a/modules/java/generator/android-21/java/org/visp/android/JavaCamera2View.java +++ b/modules/java/generator/android-21/java/org/visp/android/JavaCamera2View.java @@ -326,8 +326,8 @@ protected boolean connectCamera(int width, int height) { private class JavaCamera2Frame implements VpCameraViewFrame { @Override public VpImageUChar gray() { - if (gray == null) - gray = new VpImageUChar(mYuvFrameData,w,h,true); + if (gray == null) + gray = new VpImageUChar(mYuvFrameData,w,h,true); return gray; } @@ -370,12 +370,12 @@ public JavaCamera2Frame(byte[] Y, byte[] UV, int width, int height) { } public void release() { - // TODO openc-v had release() method here. Check whether we need to + // TODO openc-v had release() method here. Check whether we need to } private byte[] mYuvFrameData; private byte[] mUVFrameData; - private VpImageUChar gray; + private VpImageUChar gray; private VpImageRGBa rgba; private int w; private int h; diff --git a/modules/java/generator/android/java/org/visp/android/CameraBridgeViewBase.java b/modules/java/generator/android/java/org/visp/android/CameraBridgeViewBase.java index b379292007..b4fd7ad25d 100644 --- a/modules/java/generator/android/java/org/visp/android/CameraBridgeViewBase.java +++ b/modules/java/generator/android/java/org/visp/android/CameraBridgeViewBase.java @@ -396,7 +396,7 @@ protected void deliverAndDrawFrame(VpCameraViewFrame frame) { boolean bmpValid = true; if (modified != -1) { - if (mPreviewFormat == RGBA){ + if (mPreviewFormat == RGBA){ try { Utils.vpImageUCharToBitmap(modified, mCacheBitmap); } catch(Exception e) { @@ -421,7 +421,7 @@ protected void deliverAndDrawFrame(VpCameraViewFrame frame) { Canvas canvas = getHolder().lockCanvas(); if (canvas != null) { canvas.drawColor(0, android.graphics.PorterDuff.Mode.CLEAR); - Log.d(TAG, "mStretch value: " + mScale); + Log.d(TAG, "mStretch value: " + mScale); if (mScale != 0) { canvas.drawBitmap(mCacheBitmap, new Rect(0,0,mCacheBitmap.getWidth(), mCacheBitmap.getHeight()), @@ -502,7 +502,7 @@ protected int[] calculateCameraFrameSize(List supportedSizes, ListItemAccesso } } - int size[] = {calcWidth, calcHeight}; + int size[] = {calcWidth, calcHeight}; return size; } } diff --git a/modules/java/generator/android/java/org/visp/android/JavaCameraView.java b/modules/java/generator/android/java/org/visp/android/JavaCameraView.java index 473922e2ce..25e22b4cfc 100644 --- a/modules/java/generator/android/java/org/visp/android/JavaCameraView.java +++ b/modules/java/generator/android/java/org/visp/android/JavaCameraView.java @@ -288,7 +288,7 @@ protected void disconnectCamera() { @Override public void onPreviewFrame(byte[] frame, Camera arg1) { - // TODO: This should happen only in debug mode. Will do it later when the SDK is complete + // TODO: This should happen only in debug mode. Will do it later when the SDK is complete if (true) Log.d(TAG, "Preview Frame received. Frame size: " + frame.length); synchronized (this) { @@ -303,21 +303,21 @@ public void onPreviewFrame(byte[] frame, Camera arg1) { private class JavaCameraFrame implements VpCameraViewFrame { @Override public VpImageUChar gray() { - if (gray == null) - gray = new VpImageUChar(data,w,h,true); + if (gray == null) + gray = new VpImageUChar(data,w,h,true); return gray; } @Override public VpImageRGBa rgba() { - if (rgba == null){ - if (mPreviewFormat == ImageFormat.NV21) - rgba = new VpImageRGBa(data, w, h,true); - //else if (mPreviewFormat == ImageFormat.YV12) .Dropping this format for now - // Imgproc.cvtColor(mYuvFrameData, mRgba, Imgproc.COLOR_YUV2RGB_I420, 4); // COLOR_YUV2RGBA_YV12 produces inverted colors - else - throw new IllegalArgumentException("Preview Format can be NV21"); - } + if (rgba == null){ + if (mPreviewFormat == ImageFormat.NV21) + rgba = new VpImageRGBa(data, w, h,true); + //else if (mPreviewFormat == ImageFormat.YV12) .Dropping this format for now + // Imgproc.cvtColor(mYuvFrameData, mRgba, Imgproc.COLOR_YUV2RGB_I420, 4); // COLOR_YUV2RGBA_YV12 produces inverted colors + else + throw new IllegalArgumentException("Preview Format can be NV21"); + } return rgba; } @@ -330,13 +330,13 @@ public JavaCameraFrame(byte[] data, int width, int height) { } public void release() { - // open-cv had release() method here. Check whether we need to + // open-cv had release() method here. Check whether we need to } private byte[] data; - private int w; - private int h; - private VpImageUChar gray; + private int w; + private int h; + private VpImageUChar gray; private VpImageRGBa rgba; }; diff --git a/modules/java/generator/android/java/org/visp/android/Utils.java b/modules/java/generator/android/java/org/visp/android/Utils.java index f677ef88a2..cb9a666bf7 100644 --- a/modules/java/generator/android/java/org/visp/android/Utils.java +++ b/modules/java/generator/android/java/org/visp/android/Utils.java @@ -42,7 +42,7 @@ public static String exportResource(Context context, int resourceId, String dirn e.printStackTrace(); Log.e("VISP::Utils.java","Failed to export resource " + resName + ". Exception thrown: " + e); } - return null; + return null; } public static VpImageUChar loadResource(Context context, int resourceId) throws IOException @@ -61,7 +61,7 @@ public static VpImageUChar loadResource(Context context, int resourceId) throws return encoded; } - // Converts Android Bitmap to ViSP VpImageUChar + // Converts Android Bitmap to ViSP VpImageUChar public static void bitmapToVpImageUChar(Bitmap bmp, VpImageUChar image) { if (bmp == null) throw new java.lang.IllegalArgumentException("bmp == null"); @@ -70,7 +70,7 @@ public static void bitmapToVpImageUChar(Bitmap bmp, VpImageUChar image) { bitmapToVpImageUChar(bmp, image.nativeObj); } - // Converts VpImageUChar to Android Bitmap + // Converts VpImageUChar to Android Bitmap public static void vpImageUCharToBitmap(VpImageUChar image, Bitmap bmp) { if (image == null) throw new java.lang.IllegalArgumentException("image == null"); @@ -79,7 +79,7 @@ public static void vpImageUCharToBitmap(VpImageUChar image, Bitmap bmp) { vpImageUCharToBitmap(image.nativeObj, bmp); } - // Converts Android Bitmap to ViSP VpImageRGBa + // Converts Android Bitmap to ViSP VpImageRGBa public static void bitmapToVpImageRGBa(Bitmap bmp, VpImageRGBa image) { if (bmp == null) throw new java.lang.IllegalArgumentException("bmp == null"); @@ -88,7 +88,7 @@ public static void bitmapToVpImageRGBa(Bitmap bmp, VpImageRGBa image) { bitmapToVpImageRGBa(bmp, image.nativeObj); } - // Converts VpImageRGBa to Android Bitmap + // Converts VpImageRGBa to Android Bitmap public static void vpImageRGBaToBitmap(VpImageRGBa image, Bitmap bmp) { if (image == null) throw new java.lang.IllegalArgumentException("image == null"); diff --git a/modules/java/generator/android/java/org/visp/android/ViSPLoader.java.in b/modules/java/generator/android/java/org/visp/android/ViSPLoader.java.in index 3b4d9c052a..6ab57c500c 100644 --- a/modules/java/generator/android/java/org/visp/android/ViSPLoader.java.in +++ b/modules/java/generator/android/java/org/visp/android/ViSPLoader.java.in @@ -7,7 +7,7 @@ import android.content.Context; */ public class ViSPLoader { - // TODO: Add support for prev versions if visp + // TODO: Add support for prev versions if visp /** * ViSP Library version 2.10.0. */ @@ -18,7 +18,7 @@ public class ViSPLoader */ public static final String VISP_VERSION_3_0_0 = "3.0.0"; - /** + /** * ViSP Library version 3.0.1. */ public static final String VISP_VERSION_3_0_1 = "3.0.1"; diff --git a/modules/java/generator/src/java/org/visp/gui/VpDisplay.java b/modules/java/generator/src/java/org/visp/gui/VpDisplay.java index 5786571719..a409ae4ff9 100644 --- a/modules/java/generator/src/java/org/visp/gui/VpDisplay.java +++ b/modules/java/generator/src/java/org/visp/gui/VpDisplay.java @@ -23,181 +23,181 @@ import org.visp.core.VpCameraParameters; public class VpDisplay { - - // Java developers will find it easy to handle frames directly - public JFrame frame; - public BufferedImage I; - - public VpDisplay() { - frame = new JFrame(); - I = null; - } - - public static BufferedImage toBufferedImage(VpImageUChar image) { - int type = BufferedImage.TYPE_BYTE_GRAY; - byte[] b = image.getPixels(); // get all the pixels - BufferedImage I = new BufferedImage(image.cols(), image.rows(), type); - final byte[] targetPixels = ((DataBufferByte) I.getRaster().getDataBuffer()).getData(); - System.arraycopy(b, 0, targetPixels, 0, b.length); - return I; - } - - public static BufferedImage toBufferedImage(VpImageRGBa image) { - int type = BufferedImage.TYPE_4BYTE_ABGR; - byte[] b = image.getPixels(); // get all the pixels - BufferedImage I = new BufferedImage(image.cols(), image.rows(), type); - final byte[] targetPixels = ((DataBufferByte) I.getRaster().getDataBuffer()).getData(); - - // Note that in c++, visp returns RGBA format - // While java accepts ABGR, exactly the opposite for each pixel - // That's why this strange allocation - for (int j = 0; j < targetPixels.length/4; j++) { - targetPixels[4*j] = b[4*j + 3]; - targetPixels[4*j + 1] = b[4*j + 2]; - targetPixels[4*j + 2] = b[4*j + 1]; - targetPixels[4*j+3] = b[4*j]; - } - return I; - } - - public void display(VpImageUChar imageUChar) { - I = toBufferedImage(imageUChar); - } - - public void display(VpImageRGBa imageRGBa) { - I = toBufferedImage(imageRGBa); - } - - public void setTitle(String title) { - frame.setTitle(title); - } - - public void displayLine(int i1, int j1, int i2, int j2) { - displayLine(i1,j1,i2,j2,Color.RED,1); // default thickness 1. default color RED - } - - public void displayLine(int i1, int j1, int i2, int j2, Color color) { - displayLine(i1,j1,i2,j2,color,1); // default thickness 1. - } - - public void displayLine(int i1, int j1, int i2, int j2,Color color,int thickness) { - Graphics2D g = I.createGraphics(); - //g.setStroke(new BasicStroke(thickness)); - g.setColor(color); - g.drawLine(i1, j1, i2, j2); - } - - public void displayArrow(double i1, double j1, double i2, double j2,Color color,int w, int h, int thickness) { - displayArrow((int) i1, (int) j1, (int) i2, (int) j2,color,w,h,thickness); - } - - public void displayArrow(int i1, int j1, int i2, int j2,Color color,int w, int h, int thickness) { - Graphics2D g = I.createGraphics(); - //g.setStroke(new BasicStroke(thickness)); - g.setColor(color); - if (i1==i2){ - --i1; - ++i2; - } - if (j1==j2){ - --j1; - ++j2; - } - g.drawLine(i1, j1, i2, j2); - double m = (j2-j1)*1.0/(i2-i1), x1 = i2 - h/Math.sqrt(1+m*m), y1 = j2 - h*m/Math.sqrt(1+m*m); - - int x11 = (int) (x1 + m*w/(2*Math.sqrt(1+m*m))); - if ((x11-i1)*(x11-i2) > 0) - x11 = 2*i2 - x11; - - int y11 = (int) (y1 - w/(2*Math.sqrt(1+m*m))); - if ((y11-j1)*(y11-j2) > 0) - y11 = 2*j2 - y11; - g.drawLine(x11, y11, i2, j2); - - x11 = (int) (x1 - m*w/(2*Math.sqrt(1+m*m))); - if ((x11-i1)*(x11-i2) > 0) - x11 = 2*i2 - x11; - y11 = (int) (y1 + w/(2*Math.sqrt(1+m*m))); - if ((y11-j1)*(y11-j2) > 0) - y11 = 2*j2 - y11; - g.drawLine(x11, y11, i2, j2); - } - - public void displayText(String text, int i, int j) { - displayText(text, i, j, Color.GREEN, 2); // default color green, default thickness 2 - } - - public void displayText(String text, int i, int j, Color color, int thickness) { - Graphics2D g = I.createGraphics(); - //g.setStroke(new BasicStroke(thickness)); - g.setColor(color); - g.drawString(text, i, j); - } - - public void displayFrame(VpHomogeneousMatrix mat, VpCameraParameters cam, double size, int thickness) { - VpPoint o = new VpPoint(0.0, 0.0, 0.0); - VpPoint x = new VpPoint(size, 0.0, 0.0); - VpPoint y = new VpPoint(0.0, size, 0.0); - VpPoint z = new VpPoint(0.0, 0.0, size); - - o.changeFrame(mat); - o.projection(); - - x.changeFrame(mat); - x.projection(); - - y.changeFrame(mat); - y.projection(); - - z.changeFrame(mat); - z.projection(); - - displayArrow(o.get_x()*cam.get_px() + cam.get_u0(),o.get_y()*cam.get_py() + cam.get_v0(), - x.get_x()*cam.get_px() + cam.get_u0(),x.get_y()*cam.get_py() + cam.get_v0(), - Color.RED, 4 * thickness, 2 * thickness, thickness); - - displayArrow(o.get_x()*cam.get_px() + cam.get_u0(),o.get_y()*cam.get_py() + cam.get_v0(), - y.get_x()*cam.get_px() + cam.get_u0(),y.get_y()*cam.get_py() + cam.get_v0(), - Color.GREEN, 4 * thickness, 2 * thickness, thickness); - - displayArrow(o.get_x()*cam.get_px() + cam.get_u0(),o.get_y()*cam.get_py() + cam.get_v0(), - z.get_x()*cam.get_px() + cam.get_u0(),z.get_y()*cam.get_py() + cam.get_v0(), - Color.BLUE, 4 * thickness, 2 * thickness, thickness); - } - - // Flushes the output buffer associated to image - // It's necessary to use this function to see the results of any drawing - public void flush() { - ImageIcon image = new ImageIcon(I); - JLabel imageLabel = new JLabel(image); - frame = new JFrame(); - frame.add(imageLabel); - frame.pack(); // pack screen size to fit content - frame.setVisible(true); - frame.setFocusable(true); - frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE); - } - - // - // Exit on a mouse click - // - public void getClick() { - frame.addMouseListener(new MouseListener() { - @Override - public void mouseClicked(MouseEvent e) { - frame.dispatchEvent(new WindowEvent(frame, WindowEvent.WINDOW_CLOSING)); - } - - @Override - public void mouseEntered(MouseEvent arg0) {} - @Override - public void mouseExited(MouseEvent arg0) {} - @Override - public void mousePressed(MouseEvent arg0) {} - @Override - public void mouseReleased(MouseEvent arg0) {} - }); - } + + // Java developers will find it easy to handle frames directly + public JFrame frame; + public BufferedImage I; + + public VpDisplay() { + frame = new JFrame(); + I = null; + } + + public static BufferedImage toBufferedImage(VpImageUChar image) { + int type = BufferedImage.TYPE_BYTE_GRAY; + byte[] b = image.getPixels(); // get all the pixels + BufferedImage I = new BufferedImage(image.cols(), image.rows(), type); + final byte[] targetPixels = ((DataBufferByte) I.getRaster().getDataBuffer()).getData(); + System.arraycopy(b, 0, targetPixels, 0, b.length); + return I; + } + + public static BufferedImage toBufferedImage(VpImageRGBa image) { + int type = BufferedImage.TYPE_4BYTE_ABGR; + byte[] b = image.getPixels(); // get all the pixels + BufferedImage I = new BufferedImage(image.cols(), image.rows(), type); + final byte[] targetPixels = ((DataBufferByte) I.getRaster().getDataBuffer()).getData(); + + // Note that in c++, visp returns RGBA format + // While java accepts ABGR, exactly the opposite for each pixel + // That's why this strange allocation + for (int j = 0; j < targetPixels.length/4; j++) { + targetPixels[4*j] = b[4*j + 3]; + targetPixels[4*j + 1] = b[4*j + 2]; + targetPixels[4*j + 2] = b[4*j + 1]; + targetPixels[4*j+3] = b[4*j]; + } + return I; + } + + public void display(VpImageUChar imageUChar) { + I = toBufferedImage(imageUChar); + } + + public void display(VpImageRGBa imageRGBa) { + I = toBufferedImage(imageRGBa); + } + + public void setTitle(String title) { + frame.setTitle(title); + } + + public void displayLine(int i1, int j1, int i2, int j2) { + displayLine(i1,j1,i2,j2,Color.RED,1); // default thickness 1. default color RED + } + + public void displayLine(int i1, int j1, int i2, int j2, Color color) { + displayLine(i1,j1,i2,j2,color,1); // default thickness 1. + } + + public void displayLine(int i1, int j1, int i2, int j2,Color color,int thickness) { + Graphics2D g = I.createGraphics(); + //g.setStroke(new BasicStroke(thickness)); + g.setColor(color); + g.drawLine(i1, j1, i2, j2); + } + + public void displayArrow(double i1, double j1, double i2, double j2,Color color,int w, int h, int thickness) { + displayArrow((int) i1, (int) j1, (int) i2, (int) j2,color,w,h,thickness); + } + + public void displayArrow(int i1, int j1, int i2, int j2,Color color,int w, int h, int thickness) { + Graphics2D g = I.createGraphics(); + //g.setStroke(new BasicStroke(thickness)); + g.setColor(color); + if (i1==i2){ + --i1; + ++i2; + } + if (j1==j2){ + --j1; + ++j2; + } + g.drawLine(i1, j1, i2, j2); + double m = (j2-j1)*1.0/(i2-i1), x1 = i2 - h/Math.sqrt(1+m*m), y1 = j2 - h*m/Math.sqrt(1+m*m); + + int x11 = (int) (x1 + m*w/(2*Math.sqrt(1+m*m))); + if ((x11-i1)*(x11-i2) > 0) + x11 = 2*i2 - x11; + + int y11 = (int) (y1 - w/(2*Math.sqrt(1+m*m))); + if ((y11-j1)*(y11-j2) > 0) + y11 = 2*j2 - y11; + g.drawLine(x11, y11, i2, j2); + + x11 = (int) (x1 - m*w/(2*Math.sqrt(1+m*m))); + if ((x11-i1)*(x11-i2) > 0) + x11 = 2*i2 - x11; + y11 = (int) (y1 + w/(2*Math.sqrt(1+m*m))); + if ((y11-j1)*(y11-j2) > 0) + y11 = 2*j2 - y11; + g.drawLine(x11, y11, i2, j2); + } + + public void displayText(String text, int i, int j) { + displayText(text, i, j, Color.GREEN, 2); // default color green, default thickness 2 + } + + public void displayText(String text, int i, int j, Color color, int thickness) { + Graphics2D g = I.createGraphics(); + //g.setStroke(new BasicStroke(thickness)); + g.setColor(color); + g.drawString(text, i, j); + } + + public void displayFrame(VpHomogeneousMatrix mat, VpCameraParameters cam, double size, int thickness) { + VpPoint o = new VpPoint(0.0, 0.0, 0.0); + VpPoint x = new VpPoint(size, 0.0, 0.0); + VpPoint y = new VpPoint(0.0, size, 0.0); + VpPoint z = new VpPoint(0.0, 0.0, size); + + o.changeFrame(mat); + o.projection(); + + x.changeFrame(mat); + x.projection(); + + y.changeFrame(mat); + y.projection(); + + z.changeFrame(mat); + z.projection(); + + displayArrow(o.get_x()*cam.get_px() + cam.get_u0(),o.get_y()*cam.get_py() + cam.get_v0(), + x.get_x()*cam.get_px() + cam.get_u0(),x.get_y()*cam.get_py() + cam.get_v0(), + Color.RED, 4 * thickness, 2 * thickness, thickness); + + displayArrow(o.get_x()*cam.get_px() + cam.get_u0(),o.get_y()*cam.get_py() + cam.get_v0(), + y.get_x()*cam.get_px() + cam.get_u0(),y.get_y()*cam.get_py() + cam.get_v0(), + Color.GREEN, 4 * thickness, 2 * thickness, thickness); + + displayArrow(o.get_x()*cam.get_px() + cam.get_u0(),o.get_y()*cam.get_py() + cam.get_v0(), + z.get_x()*cam.get_px() + cam.get_u0(),z.get_y()*cam.get_py() + cam.get_v0(), + Color.BLUE, 4 * thickness, 2 * thickness, thickness); + } + + // Flushes the output buffer associated to image + // It's necessary to use this function to see the results of any drawing + public void flush() { + ImageIcon image = new ImageIcon(I); + JLabel imageLabel = new JLabel(image); + frame = new JFrame(); + frame.add(imageLabel); + frame.pack(); // pack screen size to fit content + frame.setVisible(true); + frame.setFocusable(true); + frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE); + } + + // + // Exit on a mouse click + // + public void getClick() { + frame.addMouseListener(new MouseListener() { + @Override + public void mouseClicked(MouseEvent e) { + frame.dispatchEvent(new WindowEvent(frame, WindowEvent.WINDOW_CLOSING)); + } + + @Override + public void mouseEntered(MouseEvent arg0) {} + @Override + public void mouseExited(MouseEvent arg0) {} + @Override + public void mousePressed(MouseEvent arg0) {} + @Override + public void mouseReleased(MouseEvent arg0) {} + }); + } } */ diff --git a/modules/java/jni/CMakeLists.txt b/modules/java/jni/CMakeLists.txt index 7d95d24266..14527f1be6 100644 --- a/modules/java/jni/CMakeLists.txt +++ b/modules/java/jni/CMakeLists.txt @@ -53,7 +53,7 @@ if(BUILD_FAT_JAVA_LIB) foreach(_dep ${__deps}) vp_target_link_libraries(${the_module} PRIVATE -Wl,-force_load "${_dep}") endforeach() - # TODO: Just check visp's file once, they had a condition I ignored + # TODO: Just check visp's file once, they had a condition I ignored elseif(((UNIX) OR (VISP_FORCE_FAT_JAVA_LIB_LD_RULES)) AND (NOT VISP_SKIP_FAT_JAVA_LIB_LD_RULES)) vp_target_link_libraries(${the_module} PRIVATE -Wl,-whole-archive ${__deps} -Wl,-no-whole-archive) else() diff --git a/modules/java/misc/core/src/java/core+VpColor.java b/modules/java/misc/core/src/java/core+VpColor.java index 177066c2b4..f2cbb1f451 100644 --- a/modules/java/misc/core/src/java/core+VpColor.java +++ b/modules/java/misc/core/src/java/core+VpColor.java @@ -1,44 +1,44 @@ package org.visp.core; -// C++: class vpColor +// C++: class vpColor public class VpColor extends VpRGBa { - public VpColor(){ - super(); + public VpColor(){ + super(); } - public VpColor(char r, char g, char b){ + public VpColor(char r, char g, char b){ super(r,g,b); } - public VpColor(int r, int g, int b){ + public VpColor(int r, int g, int b){ super((char) r,(char) g,(char) b); } - public VpColor(char value){ - super(value); - } + public VpColor(char value){ + super(value); + } - public VpColor(int value){ - super((char) value); - } + public VpColor(int value){ + super((char) value); + } - @Override - public boolean equals(Object color){ + @Override + public boolean equals(Object color){ - if (color == this) - return true; + if (color == this) + return true; - if (!(color instanceof VpColor)) - return false; + if (!(color instanceof VpColor)) + return false; - VpColor c = (VpColor) color; + VpColor c = (VpColor) color; - return (super.R == c.R) && (super.G == c.G) && (super.B == c.B); - } + return (super.R == c.R) && (super.G == c.G) && (super.B == c.B); + } - @Override - public String toString(){ - return "(" + (int) super.R + "," + (int) super.G + "," + (int) super.B + ")"; - } + @Override + public String toString(){ + return "(" + (int) super.R + "," + (int) super.G + "," + (int) super.B + ")"; + } } diff --git a/modules/java/misc/core/src/java/core+VpImageRGBa.java b/modules/java/misc/core/src/java/core+VpImageRGBa.java index 8ef1e669f8..3f42aa9b83 100644 --- a/modules/java/misc/core/src/java/core+VpImageRGBa.java +++ b/modules/java/misc/core/src/java/core+VpImageRGBa.java @@ -11,28 +11,28 @@ public VpImageRGBa(long addr){ throw new java.lang.UnsupportedOperationException("Native object address is NULL"); nativeObj = addr; } - + // C++: vpImage::vpImage() public VpImageRGBa() { - nativeObj = n_VpImageRGBa(); + nativeObj = n_VpImageRGBa(); } - + // C++: vpImage(unsigned int r, unsigned int c) public VpImageRGBa(int r, int c) { - nativeObj = n_VpImageRGBa(r,c); + nativeObj = n_VpImageRGBa(r,c); } - + // C++: vpImage(unsigned int r, unsigned int c, vpRGBa val) public VpImageRGBa(int r, int c, VpRGBa val) { - nativeObj = n_VpImageRGBa(r,c,val.R,val.G,val.B,val.A); + nativeObj = n_VpImageRGBa(r,c,val.R,val.G,val.B,val.A); } - + // C++: vpImage(Type *const array, int height, int width, bool copyData=false) // The byte array would be read from a stream public VpImageRGBa(byte[] array, int height, int width, boolean copyData) { - nativeObj = n_VpImageRGBa( array, height, width, copyData); + nativeObj = n_VpImageRGBa( array, height, width, copyData); } - + // C++: vpImage::getCols() public int cols() { return n_cols(nativeObj); @@ -45,15 +45,15 @@ public int rows() { // C++: vpImage::operator() public VpRGBa getPixel(int i, int j) { - byte res[] = n_getPixel(nativeObj, i, j); - VpRGBa val = new VpRGBa((char) (res[0] >= 0?res[0]:res[0]+256), - (char) (res[1] >= 0?res[1]:res[1]+256), - (char) (res[2] >= 0?res[2]:res[2]+256), - (char) (res[3] >= 0?res[3]:res[3]+256)); - return val; + byte res[] = n_getPixel(nativeObj, i, j); + VpRGBa val = new VpRGBa((char) (res[0] >= 0?res[0]:res[0]+256), + (char) (res[1] >= 0?res[1]:res[1]+256), + (char) (res[2] >= 0?res[2]:res[2]+256), + (char) (res[3] >= 0?res[3]:res[3]+256)); + return val; } - // C++: vpImage:: *bitmap + // C++: vpImage:: *bitmap public byte[] getPixels() { return n_getPixels(nativeObj); } @@ -61,24 +61,24 @@ public byte[] getPixels() { public long getNativeObjAddr() { return nativeObj; } - - @Override + + @Override public String toString(){ - return n_dump(nativeObj); + return n_dump(nativeObj); } - + // C++: vpImage::vpImage() private static native long n_VpImageRGBa(); - + // C++: vpImage::vpImage(int rows, int cols) private static native long n_VpImageRGBa(int rows, int cols); // C++: vpImage::vpImage(int rows, int cols, vpRGBa val) private static native long n_VpImageRGBa(int rows, int cols, char R, char G, char B, char A); - + // C++: vpImage::vpImage(Type const* array, int rows, int cols, bool copyData) private static native long n_VpImageRGBa(byte[] array, int rows, int cols, boolean copyData); - + // C++: int vpImage::cols() private static native int n_cols(long nativeObj); diff --git a/modules/java/misc/core/src/java/core+VpImageUChar.java b/modules/java/misc/core/src/java/core+VpImageUChar.java index dca5e7ae82..e29d952537 100644 --- a/modules/java/misc/core/src/java/core+VpImageUChar.java +++ b/modules/java/misc/core/src/java/core+VpImageUChar.java @@ -11,28 +11,28 @@ public VpImageUChar(long addr){ throw new java.lang.UnsupportedOperationException("Native object address is NULL"); nativeObj = addr; } - + // C++: vpImage::vpImage() public VpImageUChar() { - nativeObj = n_VpImageUChar(); + nativeObj = n_VpImageUChar(); } - + // C++: vpImage::vpImage(int rows, int cols) public VpImageUChar(int rows, int cols) { - nativeObj = n_VpImageUChar(rows,cols); + nativeObj = n_VpImageUChar(rows,cols); } - + // C++: vpImage::vpImage(int rows, int cols, uchar val) public VpImageUChar(int rows, int cols, byte b) { - nativeObj = n_VpImageUChar(rows,cols,b); + nativeObj = n_VpImageUChar(rows,cols,b); } - + // C++: vpImage(Type *const array, int height, int width, bool copyData=false) // The byte array would be read from a stream public VpImageUChar(byte[] array, int height, int width, boolean copyData) { - nativeObj = n_VpImageUChar(array, height, width, copyData); + nativeObj = n_VpImageUChar(array, height, width, copyData); } - + // C++: vpImage::vpImage::getCols() public int cols() { return n_cols(nativeObj); @@ -48,7 +48,7 @@ public int getPixel(int i, int j) { return n_getPixel(nativeObj, i, j); } - // C++: vpImage:: Type *bitmap + // C++: vpImage:: Type *bitmap public byte[] getPixels() { return n_getPixels(nativeObj); } @@ -56,24 +56,24 @@ public byte[] getPixels() { public long getNativeObjAddr() { return nativeObj; } - - @Override + + @Override public String toString(){ - return n_dump(nativeObj); + return n_dump(nativeObj); } - + // C++: vpImage::vpImage() private static native long n_VpImageUChar(); - + // C++: vpImage::vpImage(int rows, int cols) private static native long n_VpImageUChar(int rows, int cols); - + // C++: vpImage::vpImage(int rows, int cols, uchar val) private static native long n_VpImageUChar(int rows, int cols, byte b); - + // C++: vpImage::vpImage(Type const* array, int rows, int cols, bool copyData) private static native long n_VpImageUChar(byte[] array, int rows, int cols, boolean copyData); - + // C++: int vpImage::cols() private static native int n_cols(long nativeObj); diff --git a/modules/java/misc/core/src/java/core+VpRGBa.java b/modules/java/misc/core/src/java/core+VpRGBa.java index f714fed8e1..43304a1b3c 100644 --- a/modules/java/misc/core/src/java/core+VpRGBa.java +++ b/modules/java/misc/core/src/java/core+VpRGBa.java @@ -1,56 +1,56 @@ package org.visp.core; -// C++: class vrRGBa +// C++: class vrRGBa public class VpRGBa { - public char R,G,B,A; - public static final char alphaDefault = 255; + public char R,G,B,A; + public static final char alphaDefault = 255; public VpRGBa(){ - this((char) 0,(char) 0,(char) 0,alphaDefault); + this((char) 0,(char) 0,(char) 0,alphaDefault); } - public VpRGBa(char r,char g, char b){ - this(r,g,b,alphaDefault); - } - - public VpRGBa(char r,char g, char b, char a){ - this.R = r; - this.G = g; - this.B = b; - this.A = a; - } - - public VpRGBa(char v){ - this(v,v,v,v); - } - - public VpRGBa(VpRGBa c){ - this(c.R,c.G,c.B,c.A); - } - - public void add(VpRGBa other){ - this.R += other.R; - this.G += other.G; - this.B += other.B; - this.A += other.A; - } - - @Override - public String toString(){ - return "(" + (int) this.R + "," + (int) this.G + "," + (int) this.B + "," + (int) this.A + ")"; - } + public VpRGBa(char r,char g, char b){ + this(r,g,b,alphaDefault); + } + + public VpRGBa(char r,char g, char b, char a){ + this.R = r; + this.G = g; + this.B = b; + this.A = a; + } + + public VpRGBa(char v){ + this(v,v,v,v); + } + + public VpRGBa(VpRGBa c){ + this(c.R,c.G,c.B,c.A); + } + + public void add(VpRGBa other){ + this.R += other.R; + this.G += other.G; + this.B += other.B; + this.A += other.A; + } + + @Override + public String toString(){ + return "(" + (int) this.R + "," + (int) this.G + "," + (int) this.B + "," + (int) this.A + ")"; + } @Override - public boolean equals(Object o){ + public boolean equals(Object o){ - if (o == this) + if (o == this) return true; - if (!(o instanceof VpRGBa)) + if (!(o instanceof VpRGBa)) return false; - VpRGBa other = (VpRGBa) o; - return (this.R == other.R) && (this.G == other.G) && (this.B == other.B) && (this.A == other.A); - } + VpRGBa other = (VpRGBa) o; + return (this.R == other.R) && (this.G == other.G) && (this.B == other.B) && (this.A == other.A); + } } diff --git a/modules/java/misc/imgproc/src/java/imgproc+VpContour.java b/modules/java/misc/imgproc/src/java/imgproc+VpContour.java index 079a653f13..31b9c1872f 100644 --- a/modules/java/misc/imgproc/src/java/imgproc+VpContour.java +++ b/modules/java/misc/imgproc/src/java/imgproc+VpContour.java @@ -2,63 +2,62 @@ public class VpContour { - public final long nativeObj; - - public VpContour(long addr){ - if (addr == 0) - throw new java.lang.UnsupportedOperationException("Native object address is NULL"); - nativeObj = addr; + public final long nativeObj; + + public VpContour(long addr){ + if (addr == 0) + throw new java.lang.UnsupportedOperationException("Native object address is NULL"); + nativeObj = addr; } - + // C++: vpContour() public VpContour() { - nativeObj = VpContour1(); + nativeObj = VpContour1(); } - - // C++: vpContour(vpContourType type) + + // C++: vpContour(vpContourType type) public VpContour(int vpContourType) { - nativeObj = VpContour2(vpContourType); + nativeObj = VpContour2(vpContourType); } - - // C++: vpContour(vpContour contour) + + // C++: vpContour(vpContour contour) public VpContour(VpContour contour) { - nativeObj = VpContour3(contour.nativeObj); + nativeObj = VpContour3(contour.nativeObj); } - + // C++: void setParent(vpContour *parent) public void setParent(VpContour parent) { - n_setParent(nativeObj,parent.nativeObj); + n_setParent(nativeObj,parent.nativeObj); } - + @Override protected void finalize() throws Throwable { delete(nativeObj); } - - // C++: vpContour() - private static native long VpContour1(); - - // C++: vpContour(vpContourType type) + + // C++: vpContour() + private static native long VpContour1(); + + // C++: vpContour(vpContourType type) private static native long VpContour2(int vpContourType); - - // C++: vpContour(vpContour contour) + + // C++: vpContour(vpContour contour) private static native long VpContour3(long nativeObj); - + // C++: void setParent(vpContour *parent) private static native void n_setParent(long nativeObj, long parent_nativeObj); - + // native support for java finalize() private static native void delete(long nativeObj); - + public class VpContourType{ - public final static int CONTOUR_OUTER = 0; - public final static int CONTOUR_HOLE = 1; + public final static int CONTOUR_OUTER = 0; + public final static int CONTOUR_HOLE = 1; } - + public class VpContourRetrievalType{ - public final static int CONTOUR_RETR_TREE = 0; - public final static int CONTOUR_RETR_LIST = 1; - public final static int CONTOUR_RETR_EXTERNAL = 2; + public final static int CONTOUR_RETR_TREE = 0; + public final static int CONTOUR_RETR_LIST = 1; + public final static int CONTOUR_RETR_EXTERNAL = 2; } } - diff --git a/modules/java/misc/mbt/gen_dict.json b/modules/java/misc/mbt/gen_dict.json index c209d03993..62b13dc277 100644 --- a/modules/java/misc/mbt/gen_dict.json +++ b/modules/java/misc/mbt/gen_dict.json @@ -1029,7 +1029,7 @@ "jn_code": [ "" ] - }, + }, "getKltMaskBorder": { "j_code": [ "" @@ -1040,7 +1040,7 @@ "jn_code": [ "" ] - }, + }, "setKltMaskBorder": { "j_code": [ "" @@ -1051,7 +1051,7 @@ "jn_code": [ "" ] - }, + }, "getKltThresholdAcceptation": { "j_code": [ "" @@ -1062,7 +1062,7 @@ "jn_code": [ "" ] - }, + }, "setKltThresholdAcceptation": { "j_code": [ "" @@ -1073,7 +1073,7 @@ "jn_code": [ "" ] - }, + }, "setUseKltTracking": { "j_code": [ "" @@ -1084,7 +1084,7 @@ "jn_code": [ "" ] - }, + }, "getModelForDisplay" : { "j_code" : [ "//", diff --git a/modules/robot/include/visp3/robot/vpAfma4.h b/modules/robot/include/visp3/robot/vpAfma4.h index f897ec184f..12572577fd 100644 --- a/modules/robot/include/visp3/robot/vpAfma4.h +++ b/modules/robot/include/visp3/robot/vpAfma4.h @@ -116,8 +116,6 @@ class VISP_EXPORT vpAfma4 void init(void); vpHomogeneousMatrix getForwardKinematics(const vpColVector &q) const; - /* int getInverseKinematics(const vpHomogeneousMatrix & fMc, */ - /* vpColVector & q, const bool &nearest=true); */ vpHomogeneousMatrix get_fMc(const vpColVector &q) const; void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const; void get_fMc(const vpColVector &q, vpHomogeneousMatrix &fMc) const; diff --git a/modules/robot/include/visp3/robot/vpRobotException.h b/modules/robot/include/visp3/robot/vpRobotException.h index 4ce045ab65..28f1d2bc86 100644 --- a/modules/robot/include/visp3/robot/vpRobotException.h +++ b/modules/robot/include/visp3/robot/vpRobotException.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,100 +29,73 @@ * * Description: * Exception that can be emitted by the vpRobot class and its derivatives. - * -*****************************************************************************/ + */ #ifndef _vpRobotException_h_ #define _vpRobotException_h_ -/* ------------------------------------------------------------------------- - */ -/* --- INCLUDE ------------------------------------------------------------- - */ -/* ------------------------------------------------------------------------- - */ - -/* \file vpRobotException.h - \brief error that can be emitted by the vpRobot class and its derivatives +/*! + * \file vpRobotException.h + * \brief error that can be emitted by the vpRobot class and its derivatives */ -/* Classes standards. */ #include -#include /* Classe std::ostream. */ -#include /* Classe string. */ - -/* ------------------------------------------------------------------------- - */ -/* --- CLASS --------------------------------------------------------------- - */ -/* ------------------------------------------------------------------------- - */ +#include +#include /*! - \class vpRobotException - \brief Error that can be emitted by the vpRobot class and its derivatives. + * \class vpRobotException + * \brief Error that can be emitted by the vpRobot class and its derivatives. */ class VISP_EXPORT vpRobotException : public vpException { public: /*! - \brief Lists the possible error than can be emitted while calling - vpRobot member - */ - enum errorRobotCodeEnum { + * \brief Lists the possible error than can be emitted while calling + * vpRobot member + */ + enum errorRobotCodeEnum + { - /** Erreur lancee par le constructor. */ +//! Error from constructor constructionError, - /** Erreur lancee lors de la construction d'un objet CRobot - * correspondant a un robot reel si l'objet de la classe en - * question doit etre unique. */ + //! Not unique robot object construction uniqueRobotError, - /** Erreur lancee par les fonctions de commande si le - * robot n'est pas dans le bon etat au moment du passage - * d'ordre. */ + //! Wrong robot state wrongStateError, - /** Erreur lancee par les fonctions de changement d'etat - * si le changement demandee n'est pas possible. */ + //! Cannot change robot state stateModificationError, - /** Erreur generee lors d'un retour non nulle d'une fonction - * de communication de la lib Afma6. */ + //! Unable to communicate communicationError, - /** Erreur lancee apres un appel a une fonction de la lib - * bas-niveau de control de l'afma6 ayant renvoye une erreur. */ + //! Error thrown by the low level sdk lowLevelError, - /** Erreur lancee par la fonction de parsing des parametres du - * robot, si le fichier donne en entree n'est pas valide. - */ + //! Cannot parse parameters readingParametersError, - /** Erreur lancee par les methodes d'une classe qui necessite - * un appel a une fonction d'initialisation apres la - * construction si l'init n'a pas ete fait. */ + //! Cannot initialize the robot notInitializedError, - /** Erreur lancee par les fonctions decrites dans lAPI mais - * pas completement implementee. Dans ce cas, la fonction - * affiche simplement un message d'erreur avant de sortir - * par le 'throw'. - */ + //! Functionality not implemented notImplementedError, - /** Position is out of range. - */ + + //! Position is out of range positionOutOfRangeError, - /*! - Signal exception returned after SIGINT (CTRL-C), SIGBUS, SIGSEGV, SIGSEGV (CTRL-\), SIGQUIT. - */ + + //! Signal exception returned after SIGINT (CTRL-C), SIGBUS, SIGSEGV, SIGSEGV (CTRL-\), SIGQUIT. signalException }; public: + /*! + * Constructor. + */ vpRobotException(int id, const char *format, ...) { this->code = id; @@ -132,8 +104,16 @@ class VISP_EXPORT vpRobotException : public vpException setMessage(format, args); va_end(args); } - vpRobotException(int id, const std::string &msg) : vpException(id, msg) {} - explicit vpRobotException(int id) : vpException(id) {} + + /*! + * Constructor. + */ + vpRobotException(int id, const std::string &msg) : vpException(id, msg) { } + + /*! + * Constructor. + */ + explicit vpRobotException(int id) : vpException(id) { } }; #endif diff --git a/modules/robot/include/visp3/robot/vpWireFrameSimulatorTypes.h b/modules/robot/include/visp3/robot/vpWireFrameSimulatorTypes.h index bb73a3a270..3ae751a3b6 100644 --- a/modules/robot/include/visp3/robot/vpWireFrameSimulatorTypes.h +++ b/modules/robot/include/visp3/robot/vpWireFrameSimulatorTypes.h @@ -45,14 +45,14 @@ typedef char Type; typedef float Matrix[4][4]; /* - * MATRIX - * ______ + * MATRIX + * ______ * * Matrice homogene ou non. - * | Rotation | 0 | - * Matrice non homogene = | 3x3 | 0 | - * |-------------| 0 | - * | Translation | 1 | + * | Rotation | 0 | + * Matrice non homogene = | 3x3 | 0 | + * |-------------| 0 | + * | Translation | 1 | */ typedef float Matrix[4][4]; @@ -61,30 +61,30 @@ typedef float Matrix[4][4]; /* * Vertex_list : * Pour optimiser l'allocation et la liberation memoire d'une liste de - * sommets: si (nbr > DEFAULT_VSIZE) | alors ptr est alloue et libere - * dynamiquement | sinon ptr = tbl fsi; + * sommets: si (nbr > DEFAULT_VSIZE) | alors ptr est alloue et libere + * dynamiquement | sinon ptr = tbl fsi; */ typedef struct { - Index nbr; /* nombre de sommets */ - Index *ptr; /* liste dynamique */ + Index nbr; /* nombre de sommets */ + Index *ptr; /* liste dynamique */ Index tbl[DEFAULT_VSIZE]; } Vertex_list; typedef struct { - unsigned is_polygonal : 1; /* face polygonale */ - unsigned is_visible : 1; /* face affichable */ + unsigned is_polygonal : 1; /* face polygonale */ + unsigned is_visible : 1; /* face affichable */ #ifdef face_edge - Edge_list edge; /* liste d'aretes */ + Edge_list edge; /* liste d'aretes */ #endif // face_edge - Vertex_list vertex; /* liste de sommets */ + Vertex_list vertex; /* liste de sommets */ #ifdef face_normal - Vector normal; /* vecteur normal */ + Vector normal; /* vecteur normal */ #endif // face_normal } Face; typedef struct { - Index nbr; /* nombre de faces */ - Face *ptr; /* liste dynamique */ + Index nbr; /* nombre de faces */ + Face *ptr; /* liste dynamique */ } Face_list; typedef struct { @@ -92,32 +92,32 @@ typedef struct { } Point3f; typedef struct { - Index nbr; /* nombre de points */ - Point3f *ptr; /* liste dynamique */ + Index nbr; /* nombre de points */ + Point3f *ptr; /* liste dynamique */ } Point3f_list; typedef struct { - unsigned is_display : 1; /* surface affichable */ - unsigned is_polygonal : 1; /* surface polyedrique */ - Type type; /* type de la primitive */ + unsigned is_display : 1; /* surface affichable */ + unsigned is_polygonal : 1; /* surface polyedrique */ + Type type; /* type de la primitive */ #ifdef face_edge - Edge_list edge; /* liste d'aretes */ + Edge_list edge; /* liste d'aretes */ #endif // face_edge - Face_list face; /* liste de faces */ - Point3f_list point; /* points aux sommets */ + Face_list face; /* liste de faces */ + Point3f_list point; /* points aux sommets */ #ifdef face_normal - Vector_list normal; /* normales aux sommets */ + Vector_list normal; /* normales aux sommets */ #endif // face_normal } Bound; typedef struct { - Index nbr; /* nombre de surfaces */ - Bound *ptr; /* liste dynamique */ + Index nbr; /* nombre de surfaces */ + Bound *ptr; /* liste dynamique */ } Bound_list; typedef struct { - char *name; /* nom de la scene */ - Bound_list bound; /* liste de surfaces */ + char *name; /* nom de la scene */ + Bound_list bound; /* liste de surfaces */ } Bound_scene; #endif diff --git a/modules/robot/src/image-simulator/vpImageSimulator.cpp b/modules/robot/src/image-simulator/vpImageSimulator.cpp index 36b37c1660..f35cdcdc13 100644 --- a/modules/robot/src/image-simulator/vpImageSimulator.cpp +++ b/modules/robot/src/image-simulator/vpImageSimulator.cpp @@ -1241,8 +1241,8 @@ bool vpImageSimulator::getPixel(const vpImagePoint &iP, unsigned char &Ipixelpla // recuperation des coordonnes de l'intersection dans le plan objet // repere plan object : - // centre = X0_2_optim[i] (premier point definissant le plan) - // base = u:(X[1]-X[0]) et v:(X[3]-X[0]) + // centre = X0_2_optim[i] (premier point definissant le plan) + // base = u:(X[1]-X[0]) et v:(X[3]-X[0]) // ici j'ai considere que le plan est un rectangle => coordonnees sont // simplement obtenu par un produit scalaire double u = 0, v = 0; @@ -1294,8 +1294,8 @@ bool vpImageSimulator::getPixel(vpImage &Isrc, const vpImagePoint // recuperation des coordonnes de l'intersection dans le plan objet // repere plan object : - // centre = X0_2_optim[i] (premier point definissant le plan) - // base = u:(X[1]-X[0]) et v:(X[3]-X[0]) + // centre = X0_2_optim[i] (premier point definissant le plan) + // base = u:(X[1]-X[0]) et v:(X[3]-X[0]) // ici j'ai considere que le plan est un rectangle => coordonnees sont // simplement obtenu par un produit scalaire double u = 0, v = 0; @@ -1346,8 +1346,8 @@ bool vpImageSimulator::getPixel(const vpImagePoint &iP, vpRGBa &Ipixelplan) // recuperation des coordonnes de l'intersection dans le plan objet // repere plan object : - // centre = X0_2_optim[i] (premier point definissant le plan) - // base = u:(X[1]-X[0]) et v:(X[3]-X[0]) + // centre = X0_2_optim[i] (premier point definissant le plan) + // base = u:(X[1]-X[0]) et v:(X[3]-X[0]) // ici j'ai considere que le plan est un rectangle => coordonnees sont // simplement obtenu par un produit scalaire double u = 0, v = 0; @@ -1398,8 +1398,8 @@ bool vpImageSimulator::getPixel(vpImage &Isrc, const vpImagePoint &iP, v // recuperation des coordonnes de l'intersection dans le plan objet // repere plan object : - // centre = X0_2_optim[i] (premier point definissant le plan) - // base = u:(X[1]-X[0]) et v:(X[3]-X[0]) + // centre = X0_2_optim[i] (premier point definissant le plan) + // base = u:(X[1]-X[0]) et v:(X[3]-X[0]) // ici j'ai considere que le plan est un rectangle => coordonnees sont // simplement obtenu par un produit scalaire double u = 0, v = 0; diff --git a/modules/robot/src/real-robot/afma4/vpRobotAfma4.cpp b/modules/robot/src/real-robot/afma4/vpRobotAfma4.cpp index e04fe4fc72..44d55bfaa4 100644 --- a/modules/robot/src/real-robot/afma4/vpRobotAfma4.cpp +++ b/modules/robot/src/real-robot/afma4/vpRobotAfma4.cpp @@ -131,22 +131,22 @@ vpRobotAfma4::vpRobotAfma4(bool verbose) : vpAfma4(), vpRobot() { /* - #define SIGHUP 1 // hangup - #define SIGINT 2 // interrupt (rubout) - #define SIGQUIT 3 // quit (ASCII FS) - #define SIGILL 4 // illegal instruction (not reset when caught) - #define SIGTRAP 5 // trace trap (not reset when caught) - #define SIGIOT 6 // IOT instruction - #define SIGABRT 6 // used by abort, replace SIGIOT in the future - #define SIGEMT 7 // EMT instruction - #define SIGFPE 8 // floating point exception - #define SIGKILL 9 // kill (cannot be caught or ignored) - #define SIGBUS 10 // bus error - #define SIGSEGV 11 // segmentation violation - #define SIGSYS 12 // bad argument to system call - #define SIGPIPE 13 // write on a pipe with no one to read it - #define SIGALRM 14 // alarm clock - #define SIGTERM 15 // software termination signal from kill + #define SIGHUP 1 // hangup + #define SIGINT 2 // interrupt (rubout) + #define SIGQUIT 3 // quit (ASCII FS) + #define SIGILL 4 // illegal instruction (not reset when caught) + #define SIGTRAP 5 // trace trap (not reset when caught) + #define SIGIOT 6 // IOT instruction + #define SIGABRT 6 // used by abort, replace SIGIOT in the future + #define SIGEMT 7 // EMT instruction + #define SIGFPE 8 // floating point exception + #define SIGKILL 9 // kill (cannot be caught or ignored) + #define SIGBUS 10 // bus error + #define SIGSEGV 11 // segmentation violation + #define SIGSYS 12 // bad argument to system call + #define SIGPIPE 13 // write on a pipe with no one to read it + #define SIGALRM 14 // alarm clock + #define SIGTERM 15 // software termination signal from kill */ signal(SIGINT, emergencyStopAfma4); @@ -1215,7 +1215,7 @@ void vpRobotAfma4::setVelocity(const vpRobot::vpControlFrameType frame, const vp joint_vel = eJe_inverse * eVc * velocity; // printf("Vitesse art: %f %f %f %f\n", joint_vel[0], joint_vel[1], - // joint_vel[2], joint_vel[3]); + // joint_vel[2], joint_vel[3]); } // Case of the joint control where we control all the joints diff --git a/modules/robot/src/real-robot/afma4/vpServolens.cpp b/modules/robot/src/real-robot/afma4/vpServolens.cpp index 5b7754d7a9..226746015d 100644 --- a/modules/robot/src/real-robot/afma4/vpServolens.cpp +++ b/modules/robot/src/real-robot/afma4/vpServolens.cpp @@ -240,7 +240,7 @@ void vpServolens::init() const cmd = "VW0"; this->write(cmd.c_str()); - /* L'experience montre qu'une petite tempo est utile. */ + /* L'experience montre qu'une petite tempo est utile. */ vpTime::wait(500); } diff --git a/modules/robot/src/real-robot/afma6/vpAfma6.cpp b/modules/robot/src/real-robot/afma6/vpAfma6.cpp index 8d228b8864..2ead7b326c 100644 --- a/modules/robot/src/real-robot/afma6/vpAfma6.cpp +++ b/modules/robot/src/real-robot/afma6/vpAfma6.cpp @@ -353,14 +353,6 @@ void vpAfma6::init(vpAfma6::vpAfma6ToolType tool, vpCameraParameters::vpCameraPa } default: { vpERROR_TRACE("This error should not occur!"); - // vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute " - // "que les specs de la classe ont ete modifiee, " - // "et que le code n'a pas ete mis a jour " - // "correctement."); - // vpERROR_TRACE ("Verifiez les valeurs possibles du type " - // "vpAfma6::vpAfma6ToolType, et controlez que " - // "tous les cas ont ete pris en compte dans la " - // "fonction init(camera)."); break; } } @@ -640,7 +632,7 @@ int vpAfma6::getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q q_[1][5] = q_[0][5] = t - q_[0][3]; while ((q_[1][5] + vpMath::rad(2)) >= this->_joint_max[5]) - /* -> a cause du couplage 4/5 */ + /* -> a cause du couplage 4/5 */ { q_[1][5] -= vpMath::rad(10); q_[1][3] += vpMath::rad(10); @@ -656,7 +648,7 @@ int vpAfma6::getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q q_[1][3] = q_[0][3] = q[3]; q_[1][5] = q_[0][5] = q_[0][3] - t; while ((q_[1][5] + vpMath::rad(2)) >= this->_joint_max[5]) - /* -> a cause du couplage 4/5 */ + /* -> a cause du couplage 4/5 */ { q_[1][5] -= vpMath::rad(10); q_[1][3] -= vpMath::rad(10); @@ -690,7 +682,7 @@ int vpAfma6::getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q q_[1][1] = fMe[1][3] - this->_long_56 * sin(q_[1][3]); q_[0][2] = q_[1][2] = fMe[2][3]; - /* prise en compte du couplage axes 5/6 */ + /* prise en compte du couplage axes 5/6 */ q_[0][5] += this->_coupl_56 * q_[0][4]; q_[1][5] += this->_coupl_56 * q_[1][4]; diff --git a/modules/robot/src/real-robot/afma6/vpRobotAfma6.cpp b/modules/robot/src/real-robot/afma6/vpRobotAfma6.cpp index 0b28760789..65c41dd472 100644 --- a/modules/robot/src/real-robot/afma6/vpRobotAfma6.cpp +++ b/modules/robot/src/real-robot/afma6/vpRobotAfma6.cpp @@ -157,22 +157,22 @@ vpRobotAfma6::vpRobotAfma6(bool verbose) : vpAfma6(), vpRobot() { /* - #define SIGHUP 1 // hangup - #define SIGINT 2 // interrupt (rubout) - #define SIGQUIT 3 // quit (ASCII FS) - #define SIGILL 4 // illegal instruction (not reset when caught) - #define SIGTRAP 5 // trace trap (not reset when caught) - #define SIGIOT 6 // IOT instruction - #define SIGABRT 6 // used by abort, replace SIGIOT in the future - #define SIGEMT 7 // EMT instruction - #define SIGFPE 8 // floating point exception - #define SIGKILL 9 // kill (cannot be caught or ignored) - #define SIGBUS 10 // bus error - #define SIGSEGV 11 // segmentation violation - #define SIGSYS 12 // bad argument to system call - #define SIGPIPE 13 // write on a pipe with no one to read it - #define SIGALRM 14 // alarm clock - #define SIGTERM 15 // software termination signal from kill + #define SIGHUP 1 // hangup + #define SIGINT 2 // interrupt (rubout) + #define SIGQUIT 3 // quit (ASCII FS) + #define SIGILL 4 // illegal instruction (not reset when caught) + #define SIGTRAP 5 // trace trap (not reset when caught) + #define SIGIOT 6 // IOT instruction + #define SIGABRT 6 // used by abort, replace SIGIOT in the future + #define SIGEMT 7 // EMT instruction + #define SIGFPE 8 // floating point exception + #define SIGKILL 9 // kill (cannot be caught or ignored) + #define SIGBUS 10 // bus error + #define SIGSEGV 11 // segmentation violation + #define SIGSYS 12 // bad argument to system call + #define SIGPIPE 13 // write on a pipe with no one to read it + #define SIGALRM 14 // alarm clock + #define SIGTERM 15 // software termination signal from kill */ signal(SIGINT, emergencyStopAfma6); diff --git a/modules/robot/src/real-robot/biclops/vpRobotBiclops.cpp b/modules/robot/src/real-robot/biclops/vpRobotBiclops.cpp index 300b02967c..dbe18ed09d 100644 --- a/modules/robot/src/real-robot/biclops/vpRobotBiclops.cpp +++ b/modules/robot/src/real-robot/biclops/vpRobotBiclops.cpp @@ -739,14 +739,6 @@ void vpRobotBiclops::setPosition(const vpRobot::vpControlFrameType frame, const break; } - // test if position reachable - // if ( (fabs(q[0]) > vpBiclops::panJointLimit) || - // (fabs(q[1]) > vpBiclops::tiltJointLimit) ) { - // vpERROR_TRACE ("Positioning error."); - // throw vpRobotException (vpRobotException::wrongStateError, - // "Positioning error."); - // } - vpDEBUG_TRACE(12, "Lock mutex vpEndThread_mutex"); pthread_mutex_lock(&vpEndThread_mutex); controller.setPosition(q, positioningVelocity); diff --git a/modules/robot/src/real-robot/franka/vpJointVelTrajGenerator_impl.cpp b/modules/robot/src/real-robot/franka/vpJointVelTrajGenerator_impl.cpp index 159c74f0ec..82d6b34b0a 100644 --- a/modules/robot/src/real-robot/franka/vpJointVelTrajGenerator_impl.cpp +++ b/modules/robot/src/real-robot/franka/vpJointVelTrajGenerator_impl.cpp @@ -483,9 +483,9 @@ void vpJointVelTrajGenerator::applyVel(const std::array &dq_des, std: /* * Compute new command in case of - * - acceleration - * - deceleration - * - stop + * - acceleration + * - deceleration + * - stop */ for (size_t i = 0; i < m_njoints; i++) { // Security joint limit diff --git a/modules/robot/src/real-robot/ptu46/vpPtu46.cpp b/modules/robot/src/real-robot/ptu46/vpPtu46.cpp index 7e532aea82..9d904eea5c 100644 --- a/modules/robot/src/real-robot/ptu46/vpPtu46.cpp +++ b/modules/robot/src/real-robot/ptu46/vpPtu46.cpp @@ -41,7 +41,7 @@ #include #include -/* Inclusion des fichiers standards. */ +/* Inclusion des fichiers standards. */ #include #include diff --git a/modules/robot/src/real-robot/viper/vpRobotViper650.cpp b/modules/robot/src/real-robot/viper/vpRobotViper650.cpp index 5a26884158..457735cc62 100644 --- a/modules/robot/src/real-robot/viper/vpRobotViper650.cpp +++ b/modules/robot/src/real-robot/viper/vpRobotViper650.cpp @@ -175,22 +175,22 @@ vpRobotViper650::vpRobotViper650(bool verbose) : vpViper650(), vpRobot() { /* - #define SIGHUP 1 // hangup - #define SIGINT 2 // interrupt (rubout) - #define SIGQUIT 3 // quit (ASCII FS) - #define SIGILL 4 // illegal instruction (not reset when caught) - #define SIGTRAP 5 // trace trap (not reset when caught) - #define SIGIOT 6 // IOT instruction - #define SIGABRT 6 // used by abort, replace SIGIOT in the future - #define SIGEMT 7 // EMT instruction - #define SIGFPE 8 // floating point exception - #define SIGKILL 9 // kill (cannot be caught or ignored) - #define SIGBUS 10 // bus error - #define SIGSEGV 11 // segmentation violation - #define SIGSYS 12 // bad argument to system call - #define SIGPIPE 13 // write on a pipe with no one to read it - #define SIGALRM 14 // alarm clock - #define SIGTERM 15 // software termination signal from kill + #define SIGHUP 1 // hangup + #define SIGINT 2 // interrupt (rubout) + #define SIGQUIT 3 // quit (ASCII FS) + #define SIGILL 4 // illegal instruction (not reset when caught) + #define SIGTRAP 5 // trace trap (not reset when caught) + #define SIGIOT 6 // IOT instruction + #define SIGABRT 6 // used by abort, replace SIGIOT in the future + #define SIGEMT 7 // EMT instruction + #define SIGFPE 8 // floating point exception + #define SIGKILL 9 // kill (cannot be caught or ignored) + #define SIGBUS 10 // bus error + #define SIGSEGV 11 // segmentation violation + #define SIGSYS 12 // bad argument to system call + #define SIGPIPE 13 // write on a pipe with no one to read it + #define SIGALRM 14 // alarm clock + #define SIGTERM 15 // software termination signal from kill */ signal(SIGINT, emergencyStopViper650); diff --git a/modules/robot/src/real-robot/viper/vpRobotViper850.cpp b/modules/robot/src/real-robot/viper/vpRobotViper850.cpp index fc7e3b9919..2d6d369b8d 100644 --- a/modules/robot/src/real-robot/viper/vpRobotViper850.cpp +++ b/modules/robot/src/real-robot/viper/vpRobotViper850.cpp @@ -174,22 +174,22 @@ vpRobotViper850::vpRobotViper850(bool verbose) : vpViper850(), vpRobot() { /* - #define SIGHUP 1 // hangup - #define SIGINT 2 // interrupt (rubout) - #define SIGQUIT 3 // quit (ASCII FS) - #define SIGILL 4 // illegal instruction (not reset when caught) - #define SIGTRAP 5 // trace trap (not reset when caught) - #define SIGIOT 6 // IOT instruction - #define SIGABRT 6 // used by abort, replace SIGIOT in the future - #define SIGEMT 7 // EMT instruction - #define SIGFPE 8 // floating point exception - #define SIGKILL 9 // kill (cannot be caught or ignored) - #define SIGBUS 10 // bus error - #define SIGSEGV 11 // segmentation violation - #define SIGSYS 12 // bad argument to system call - #define SIGPIPE 13 // write on a pipe with no one to read it - #define SIGALRM 14 // alarm clock - #define SIGTERM 15 // software termination signal from kill + #define SIGHUP 1 // hangup + #define SIGINT 2 // interrupt (rubout) + #define SIGQUIT 3 // quit (ASCII FS) + #define SIGILL 4 // illegal instruction (not reset when caught) + #define SIGTRAP 5 // trace trap (not reset when caught) + #define SIGIOT 6 // IOT instruction + #define SIGABRT 6 // used by abort, replace SIGIOT in the future + #define SIGEMT 7 // EMT instruction + #define SIGFPE 8 // floating point exception + #define SIGKILL 9 // kill (cannot be caught or ignored) + #define SIGBUS 10 // bus error + #define SIGSEGV 11 // segmentation violation + #define SIGSYS 12 // bad argument to system call + #define SIGPIPE 13 // write on a pipe with no one to read it + #define SIGALRM 14 // alarm clock + #define SIGTERM 15 // software termination signal from kill */ signal(SIGINT, emergencyStopViper850); diff --git a/modules/robot/src/real-robot/viper/vpViper.cpp b/modules/robot/src/real-robot/viper/vpViper.cpp index 80c22b7061..84c7f2f6fd 100644 --- a/modules/robot/src/real-robot/viper/vpViper.cpp +++ b/modules/robot/src/real-robot/viper/vpViper.cpp @@ -463,7 +463,6 @@ unsigned int vpViper::getInverseKinematicsWrist(const vpHomogeneousMatrix &fMw, if (ok[i] == true) { nbsol++; sol = i; - // dist[i] = vpColVector::distance(q, q_sol[i]); vpColVector weight(6); weight = 1; weight[0] = 8; @@ -479,13 +478,11 @@ unsigned int vpViper::getInverseKinematicsWrist(const vpHomogeneousMatrix &fMw, if (fabs(rought_dist + 2 * M_PI) < fabs(rought_dist)) modulo_dist = rought_dist + 2 * M_PI; } - // std::cout << "dist " << i << ": " << rought_dist << " modulo: " << - // modulo_dist << std::endl; + dist[i] += weight[j] * vpMath::sqr(modulo_dist); } } - // std::cout << "sol " << i << " [" << ok[i] << "] dist: " << dist[i] << - // " q: " << q_sol[i].t() << std::endl; + } // std::cout << "dist: " << dist.t() << std::endl; if (nbsol) { @@ -497,8 +494,6 @@ unsigned int vpViper::getInverseKinematicsWrist(const vpHomogeneousMatrix &fMw, // Update the inverse kinematics solution q = q_sol[sol]; - // std::cout << "Nearest solution (" << sol << ") with distance (" - // << dist[sol] << "): " << q_sol[sol].t() << std::endl; } return nbsol; } @@ -722,11 +717,6 @@ void vpViper::get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const // if positions are motor position. // double q6 = q[5] + c56 * q[4]; - // std::cout << "q6 motor: " << q[5] << " rad " - // << vpMath::deg(q[5]) << " deg" << std::endl; - // std::cout << "q6 joint: " << q6 << " rad " - // << vpMath::deg(q6) << " deg" << std::endl; - double c1 = cos(q1); double s1 = sin(q1); double c2 = cos(q2); @@ -817,11 +807,6 @@ void vpViper::get_fMw(const vpColVector &q, vpHomogeneousMatrix &fMw) const // if positions are motor position. // double q6 = q[5] + c56 * q[4]; - // std::cout << "q6 motor: " << q[5] << " rad " - // << vpMath::deg(q[5]) << " deg" << std::endl; - // std::cout << "q6 joint: " << q6 << " rad " - // << vpMath::deg(q6) << " deg" << std::endl; - double c1 = cos(q1); double s1 = sin(q1); double c2 = cos(q2); diff --git a/modules/robot/src/real-robot/viper/vpViper650.cpp b/modules/robot/src/real-robot/viper/vpViper650.cpp index 7ce59384ab..48869f951b 100644 --- a/modules/robot/src/real-robot/viper/vpViper650.cpp +++ b/modules/robot/src/real-robot/viper/vpViper650.cpp @@ -251,14 +251,6 @@ void vpViper650::init(vpViper650::vpToolType tool, vpCameraParameters::vpCameraP } default: { vpERROR_TRACE("This error should not occur!"); - // vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute " - // "que les specs de la classe ont ete modifiee, " - // "et que le code n'a pas ete mis a jour " - // "correctement."); - // vpERROR_TRACE ("Verifiez les valeurs possibles du type " - // "vpViper650::vpViper650ToolType, et controlez que " - // "tous les cas ont ete pris en compte dans la " - // "fonction init(camera)."); break; } } diff --git a/modules/robot/src/real-robot/viper/vpViper850.cpp b/modules/robot/src/real-robot/viper/vpViper850.cpp index b073d36b6f..6d1b1568d2 100644 --- a/modules/robot/src/real-robot/viper/vpViper850.cpp +++ b/modules/robot/src/real-robot/viper/vpViper850.cpp @@ -252,14 +252,6 @@ void vpViper850::init(vpViper850::vpToolType tool, vpCameraParameters::vpCameraP } default: { vpERROR_TRACE("This error should not occur!"); - // vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute " - // "que les specs de la classe ont ete modifiee, " - // "et que le code n'a pas ete mis a jour " - // "correctement."); - // vpERROR_TRACE ("Verifiez les valeurs possibles du type " - // "vpViper850::vpViper850ToolType, et controlez que " - // "tous les cas ont ete pris en compte dans la " - // "fonction init(camera)."); break; } } diff --git a/modules/robot/src/wireframe-simulator/vpArit.cpp b/modules/robot/src/wireframe-simulator/vpArit.cpp index 06cc28af33..6fb47be47d 100644 --- a/modules/robot/src/wireframe-simulator/vpArit.cpp +++ b/modules/robot/src/wireframe-simulator/vpArit.cpp @@ -47,8 +47,8 @@ /* * La procedure "fprintf_matrix" affiche une matrice sur un fichier. * Entree : - * fp Fichier en sortie. - * m Matrice a ecrire. + * fp Fichier en sortie. + * m Matrice a ecrire. */ void fprintf_matrix(FILE *fp, Matrix m) { @@ -63,7 +63,7 @@ void fprintf_matrix(FILE *fp, Matrix m) /* * La procedure "ident_matrix" initialise la matrice par la matrice identite. * Entree : - * m Matrice a initialiser. + * m Matrice a initialiser. */ void ident_matrix(Matrix m) { @@ -74,19 +74,19 @@ void ident_matrix(Matrix m) /* * Version moins rapide. * - * int i, j; + * int i, j; * - * for (i = 0; i < 4; i++) - * for (j = 0; j < 4; j++) - * m[i][j] = (i == j) ? 1.0 : 0.0; + * for (i = 0; i < 4; i++) + * for (j = 0; j < 4; j++) + * m[i][j] = (i == j) ? 1.0 : 0.0; */ } /* * La procedure "premult_matrix" pre multiplie la matrice par la seconde. * Entree : - * a Premiere matrice du produit a = b * a. - * b Seconde matrice du produit. + * a Premiere matrice du produit a = b * a. + * b Seconde matrice du produit. */ void premult_matrix(Matrix a, Matrix b) { @@ -104,8 +104,8 @@ void premult_matrix(Matrix a, Matrix b) * La procedure "premult3_matrix" premultiplie la matrice par une matrice 3x3. * Note : La procedure "premult3_matrix" optimise "premutl_matrix". * Entree : - * a Premiere matrice du produit a = b * a. - * b Seconde matrice du produit 3x3. + * a Premiere matrice du produit a = b * a. + * b Seconde matrice du produit 3x3. */ void premult3_matrix(Matrix a, Matrix b) { @@ -122,8 +122,8 @@ void premult3_matrix(Matrix a, Matrix b) /* * La procedure "prescale_matrix" premultiplie la matrice par l'homothetie. * Entree : - * m Matrice a multiplier m = vp * m. - * vp Vecteur d'homothetie. + * m Matrice a multiplier m = vp * m. + * vp Vecteur d'homothetie. */ void prescale_matrix(Matrix m, Vector *vp) { @@ -139,8 +139,8 @@ void prescale_matrix(Matrix m, Vector *vp) /* * La procedure "pretrans_matrix" premultiplie la matrice par la translation. * Entree : - * m Matrice a multiplier m = vp * m. - * vp Vecteur de translation. + * m Matrice a multiplier m = vp * m. + * vp Vecteur de translation. */ void pretrans_matrix(Matrix m, Vector *vp) { @@ -154,8 +154,8 @@ void pretrans_matrix(Matrix m, Vector *vp) * La procedure "postleft_matrix" postmultiplie la matrice * par une matrice gauche sur un des axes. * Entree : - * m Matrice a rendre gauche m = m * left. - * axis Axe de la matrice gauche 'x', 'y' ou 'z'. + * m Matrice a rendre gauche m = m * left. + * axis Axe de la matrice gauche 'x', 'y' ou 'z'. */ void postleft_matrix(Matrix m, char axis) { @@ -186,8 +186,8 @@ void postleft_matrix(Matrix m, char axis) /* * La procedure "postmult_matrix" post multiplie la matrice par la seconde. * Entree : - * a Premiere matrice du produit a = a * b. - * b Seconde matrice du produit. + * a Premiere matrice du produit a = a * b. + * b Seconde matrice du produit. */ void postmult_matrix(Matrix a, Matrix b) { @@ -205,8 +205,8 @@ void postmult_matrix(Matrix a, Matrix b) * La procedure "postmult3_matrix" postmultiplie la matrice par une matrice * 3x3. Note : La procedure "postmult3_matrix" optimise "postmutl_matrix". * Entree : - * a Premiere matrice du produit a = a * b. - * b Seconde matrice du produit 3x3. + * a Premiere matrice du produit a = a * b. + * b Seconde matrice du produit 3x3. */ void postmult3_matrix(Matrix a, Matrix b) { @@ -223,8 +223,8 @@ void postmult3_matrix(Matrix a, Matrix b) /* * La procedure "postscale_matrix" post multiplie la matrice par l'homothetie. * Entree : - * m Matrice a multiplier m = m * vp. - * vp Vecteur d'homothetie. + * m Matrice a multiplier m = m * vp. + * vp Vecteur d'homothetie. */ void postscale_matrix(Matrix m, Vector *vp) { @@ -239,7 +239,7 @@ void postscale_matrix(Matrix m, Vector *vp) /* * La procedure "posttrans_matrix" post mutiplie la matrice par la - * translation. Entree : m Matrice a multiplier m = m * vp. vp + * translation. Entree : m Matrice a multiplier m = m * vp. vp * Vecteur de translation. */ void posttrans_matrix(Matrix m, Vector *vp) @@ -256,7 +256,7 @@ void posttrans_matrix(Matrix m, Vector *vp) /* * La procedure "transpose_matrix" transpose la matrice. * Entree : - * m Matrice a transposer. + * m Matrice a transposer. */ void transpose_matrix(Matrix m) { @@ -272,13 +272,13 @@ void transpose_matrix(Matrix m) * La procedure "cosin_to_angle" calcule un angle a partir d'un cosinus * et d'un sinus. * Entree : - * ca, sa Cosinus et Sinus de l'angle. + * ca, sa Cosinus et Sinus de l'angle. * Sortie : - * Angle en radians. + * Angle en radians. */ float cosin_to_angle(float ca, float sa) { - float a; /* angle a calculer */ + float a; /* angle a calculer */ if (FABS(ca) < M_EPSILON) { a = (sa > (float)0.0) ? (float)M_PI_2 : (float)(-M_PI_2); @@ -294,27 +294,27 @@ float cosin_to_angle(float ca, float sa) * La procedure "cosin_to_lut" precalcule les tables des "cosinus" et "sinus". * Les tables possedent "2 ** level" entrees pour M_PI_2 radians. * Entree : - * level Niveau de decomposition. - * coslut Table pour la fonction "cosinus". - * sinlut Table pour la fonction "sinus". + * level Niveau de decomposition. + * coslut Table pour la fonction "cosinus". + * sinlut Table pour la fonction "sinus". */ void cosin_to_lut(Index level, float *coslut, float *sinlut) { int i; int i_pi_2 = TWO_POWER(level); - int quad; /* quadrant courant */ - double a; /* angle courant */ + int quad; /* quadrant courant */ + double a; /* angle courant */ double step = M_PI_2 / (double)i_pi_2; quad = 0; coslut[quad] = 1.0; - sinlut[quad] = 0.0; /* 0 */ + sinlut[quad] = 0.0; /* 0 */ quad += i_pi_2; coslut[quad] = 0.0; - sinlut[quad] = 1.0; /* PI/2 */ + sinlut[quad] = 1.0; /* PI/2 */ quad += i_pi_2; coslut[quad] = -1.0; - sinlut[quad] = 0.0; /* PI */ + sinlut[quad] = 0.0; /* PI */ quad += i_pi_2; coslut[quad] = 0.0; sinlut[quad] = -1.0; /* 3PI/2*/ @@ -322,18 +322,18 @@ void cosin_to_lut(Index level, float *coslut, float *sinlut) for (i = 1, a = step; i < i_pi_2; i++, a += step) { float ca = (float)cos(a); quad = 0; - coslut[quad + i] = ca; /* cos(a) */ + coslut[quad + i] = ca; /* cos(a) */ quad += i_pi_2; - sinlut[quad - i] = ca; /* sin(PI/2-a) */ - sinlut[quad + i] = ca; /* sin(PI/2+a) */ + sinlut[quad - i] = ca; /* sin(PI/2-a) */ + sinlut[quad + i] = ca; /* sin(PI/2+a) */ quad += i_pi_2; - coslut[quad - i] = -ca; /* cos(PI-a) */ - coslut[quad + i] = -ca; /* cos(PI+a) */ + coslut[quad - i] = -ca; /* cos(PI-a) */ + coslut[quad + i] = -ca; /* cos(PI+a) */ quad += i_pi_2; - sinlut[quad - i] = -ca; /* sin(3PI/2-a) */ - sinlut[quad + i] = -ca; /* sin(3PI/2+a) */ + sinlut[quad - i] = -ca; /* sin(3PI/2-a) */ + sinlut[quad + i] = -ca; /* sin(3PI/2+a) */ quad += i_pi_2; - coslut[quad - i] = ca; /* cos(2PI-a) */ + coslut[quad - i] = ca; /* cos(2PI-a) */ } } @@ -341,13 +341,13 @@ void cosin_to_lut(Index level, float *coslut, float *sinlut) * La procedure "norm_vector" normalise le vecteur. * Si la norme est nulle la normalisation n'est pas effectuee. * Entree : - * vp Le vecteur a norme. + * vp Le vecteur a norme. * Sortie : - * La norme du vecteur. + * La norme du vecteur. */ float norm_vector(Vector *vp) { - float norm; /* norme du vecteur */ + float norm; /* norme du vecteur */ if ((norm = (float)sqrt((double)DOT_PRODUCT(*vp, *vp))) > M_EPSILON) { vp->x /= norm; @@ -364,14 +364,14 @@ float norm_vector(Vector *vp) * La procedure "plane_norme" calcule le vecteur norme orthogonal au plan * defini par les 3 points. * Entree : - * np Le vecteur norme orthogonal au plan. - * ap, bp, cp Points formant un repere du plan. + * np Le vecteur norme orthogonal au plan. + * ap, bp, cp Points formant un repere du plan. */ void plane_norme(Vector *np, Point3f *ap, Point3f *bp, Point3f *cp) { Vector u, v; - DIF_COORD3(u, *bp, *ap); /* base orthonorme (ap, u, v) */ + DIF_COORD3(u, *bp, *ap); /* base orthonorme (ap, u, v) */ DIF_COORD3(v, *cp, *ap); norm_vector(&u); norm_vector(&v); @@ -382,9 +382,9 @@ void plane_norme(Vector *np, Point3f *ap, Point3f *bp, Point3f *cp) * La procedure "point_matrix" deplace un point 3D dans un espace 4D. * Une matrice homogene 4x4 effectue le changement de repere. * Entree : - * p4 Point homogene resultat = p3 x m. - * p3 Point a deplacer. - * m Matrice de changement de repere. + * p4 Point homogene resultat = p3 x m. + * p3 Point a deplacer. + * m Matrice de changement de repere. */ void point_matrix(Point4f *p4, Point3f *p3, Matrix m) { @@ -400,13 +400,13 @@ void point_matrix(Point4f *p4, Point3f *p3, Matrix m) * La procedure "point_3D_3D" deplace un tableau de points 3D dans un espace * 3D. Une matrice 4x3 effectue le changement de repere. La quatrieme colonne * de la matrice vaut [0, 0, 0, 1] et n'est pas utilisee. Entree : ip - * Tableau de points 3D a deplacer. size Taille du tableau - * "ip". m Matrice de changement de repere. Entree/Sortie : op + * Tableau de points 3D a deplacer. size Taille du tableau + * "ip". m Matrice de changement de repere. Entree/Sortie : op * Tableau de points 3D resultat. */ void point_3D_3D(Point3f *ip, int size, Matrix m, Point3f *op) { - Point3f *pend = ip + size; /* borne de ip */ + Point3f *pend = ip + size; /* borne de ip */ for (; ip < pend; ip++, op++) { float x = ip->x; @@ -422,13 +422,13 @@ void point_3D_3D(Point3f *ip, int size, Matrix m, Point3f *op) /* * La procedure "point_3D_4D" deplace un tableau de points 3D dans un espace * 4D. Une matrice homogene 4x4 effectue le changement de repere. Entree : p3 - * Tableau de points 3D a deplacer. size Taille du tableau - * "p3". m Matrice de changement de repere. Entree/Sortie : p4 + * Tableau de points 3D a deplacer. size Taille du tableau + * "p3". m Matrice de changement de repere. Entree/Sortie : p4 * Tableau de points 4D resultat. */ void point_3D_4D(Point3f *p3, int size, Matrix m, Point4f *p4) { - Point3f *pend = p3 + size; /* borne de p3 */ + Point3f *pend = p3 + size; /* borne de p3 */ for (; p3 < pend; p3++, p4++) { float x = p3->x; @@ -446,18 +446,18 @@ void point_3D_4D(Point3f *p3, int size, Matrix m, Point4f *p4) * La procedure "rotate_vector" transforme le vecteur * par la rotation de sens trigonometrique d'angle et d'axe donnes. * Entree : - * vp Vecteur a transformer. - * a Angle de rotation en degres. - * axis Vecteur directeur de l'axe de rotation. + * vp Vecteur a transformer. + * a Angle de rotation en degres. + * axis Vecteur directeur de l'axe de rotation. */ void rotate_vector(Vector *vp, float a, Vector *axis) { Vector n, u, v, cross; float f; - a *= (float)M_PI / (float)180.0; /* passage en radians */ + a *= (float)M_PI / (float)180.0; /* passage en radians */ - n = *axis; /* norme le vecteur directeur */ + n = *axis; /* norme le vecteur directeur */ norm_vector(&n); /* @@ -472,16 +472,16 @@ void rotate_vector(Vector *vp, float a, Vector *axis) */ f = DOT_PRODUCT(*vp, n); u = n; - MUL_COORD3(u, f, f, f); /* (vp.n) * n */ + MUL_COORD3(u, f, f, f); /* (vp.n) * n */ - DIF_COORD3(v, *vp, u); /* calcule "v" */ + DIF_COORD3(v, *vp, u); /* calcule "v" */ f = (float)cos((double)a); - MUL_COORD3(v, f, f, f); /* v * cos(a) */ + MUL_COORD3(v, f, f, f); /* v * cos(a) */ CROSS_PRODUCT(cross, n, *vp); f = (float)sin((double)a); - MUL_COORD3(cross, f, f, f); /* (n^v) * sin(a) */ + MUL_COORD3(cross, f, f, f); /* (n^v) * sin(a) */ SET_COORD3(*vp, u.x + v.x + cross.x, u.y + v.y + cross.y, u.z + v.z + cross.z); } @@ -490,19 +490,19 @@ void rotate_vector(Vector *vp, float a, Vector *axis) * La procedure "upright_vector" calcule un vecteur perpendiculaire. * Les vecteurs ont un produit scalaire nul. * Entree : - * vp Vecteur origine. + * vp Vecteur origine. * Entree/Sortie : - * up Vecteur perpendiculaire a vp. + * up Vecteur perpendiculaire a vp. */ void upright_vector(Vector *vp, Vector *up) { - if (FABS(vp->z) > M_EPSILON) { /* x et y sont fixes */ + if (FABS(vp->z) > M_EPSILON) { /* x et y sont fixes */ up->z = -(vp->x + vp->y) / vp->z; up->x = up->y = 1.0; - } else if (FABS(vp->y) > M_EPSILON) { /* x et z sont fixes */ + } else if (FABS(vp->y) > M_EPSILON) { /* x et z sont fixes */ up->y = -(vp->x + vp->z) / vp->y; up->x = up->z = 1.0; - } else if (FABS(vp->x) > M_EPSILON) { /* y et z sont fixes */ + } else if (FABS(vp->x) > M_EPSILON) { /* y et z sont fixes */ up->x = -(vp->y + vp->z) / vp->x; up->y = up->z = 1.0; } else { @@ -518,8 +518,8 @@ void upright_vector(Vector *vp, Vector *up) * Si M est la matrice, et P la position : M = R.Sid.T, P = (R,Sid,T). * On suppose que la matrice de rotation 3x3 de M est unitaire. * Entree : - * m Matrice de rotation et de translation. - * pp Position a initialiser. + * m Matrice de rotation et de translation. + * pp Position a initialiser. */ void Matrix_to_Position(Matrix m, AritPosition *pp) { @@ -532,15 +532,15 @@ void Matrix_to_Position(Matrix m, AritPosition *pp) * La procedure "Matrix_to_Rotate" initialise la rotation par la matrice. * Si M est la matrice, si R est la matrice de rotation : * - * | m00 m01 m02 0 | - * M = Rx.Ry.Rz = | m10 m11 m12 0 | - * | m20 m21 m22 0 | - * | 0 0 0 1 | + * | m00 m01 m02 0 | + * M = Rx.Ry.Rz = | m10 m11 m12 0 | + * | m20 m21 m22 0 | + * | 0 0 0 1 | * - * et m00 = cy.cz m01 = cy.sz m02 = -sy - * m10 = sx.sy.cz-cx.sz m11 = sx.sy.sz+cx.cz m12 = sx.cy - * m20 = cx.sy.cz+sx.sz m21 = cx.sy.sz-sx.cz m22 = cx.cy - * avec ci = cos Oi et si = sin Oi. + * et m00 = cy.cz m01 = cy.sz m02 = -sy + * m10 = sx.sy.cz-cx.sz m11 = sx.sy.sz+cx.cz m12 = sx.cy + * m20 = cx.sy.cz+sx.sz m21 = cx.sy.sz-sx.cz m22 = cx.cy + * avec ci = cos Oi et si = sin Oi. * * R = Rx.Ry.Rz * Rx rotation autour de Ox d'angle O1 @@ -548,11 +548,11 @@ void Matrix_to_Position(Matrix m, AritPosition *pp) * Rz rotation autour de Oz d'angle O3 * * Singularite : si |ry| == 90 degres alors rz = 0, - * soit une rotation d'axe 0z et d'angle "rx + rz". + * soit une rotation d'axe 0z et d'angle "rx + rz". * * Entree : - * m Matrice contenant la composition des rotations. - * vp Rotations par rapport aux axes d'un repere droit en degres. + * m Matrice contenant la composition des rotations. + * vp Rotations par rapport aux axes d'un repere droit en degres. */ void Matrix_to_Rotate(Matrix m, Vector *vp) { @@ -568,13 +568,13 @@ void Matrix_to_Rotate(Matrix m, Vector *vp) cx = m[2][2] / cy; SET_COORD3(*vp, cosin_to_angle(cx, sx), cosin_to_angle(cy, sy), cosin_to_angle(cz, sz)); - } else { /* RZ = 0 => Ry = +/- 90 degres */ + } else { /* RZ = 0 => Ry = +/- 90 degres */ sx = m[1][1]; cx = -m[2][1]; SET_COORD3(*vp, cosin_to_angle(cx, sx), (sy > (float)0.0) ? (float)M_PI_2 : (float)(-M_PI_2), (float)0.0); } - vp->x *= (float)180.0 / (float)M_PI; /* passage en degres */ + vp->x *= (float)180.0 / (float)M_PI; /* passage en degres */ vp->y *= (float)180.0 / (float)M_PI; vp->z *= (float)180.0 / (float)M_PI; } @@ -583,14 +583,14 @@ void Matrix_to_Rotate(Matrix m, Vector *vp) * La procedure "Position_to_Matrix" initialise la matrice par la position. * Matrice resultat : M = Sx.Sy.Sz.Rx.Ry.Rz.Tx.Ty.Tz * Entree : - * pp Position de reference. - * m Matrice a initialiser. + * pp Position de reference. + * m Matrice a initialiser. */ void Position_to_Matrix(AritPosition *pp, Matrix m) { - Rotate_to_Matrix(&pp->rotate, m); /* rotation */ - prescale_matrix(m, &pp->scale); /* homothetie */ - m[3][0] = pp->translate.x; /* translation */ + Rotate_to_Matrix(&pp->rotate, m); /* rotation */ + prescale_matrix(m, &pp->scale); /* homothetie */ + m[3][0] = pp->translate.x; /* translation */ m[3][1] = pp->translate.y; m[3][2] = pp->translate.z; } @@ -598,26 +598,26 @@ void Position_to_Matrix(AritPosition *pp, Matrix m) /* * La procedure "Rotate_to_Matrix" initialise la matrice par la rotation. * - * | m00 m01 m02 0 | - * M = Rx.Ry.Rz = | m10 m11 m12 0 | - * | m20 m21 m22 0 | - * | 0 0 0 1 | + * | m00 m01 m02 0 | + * M = Rx.Ry.Rz = | m10 m11 m12 0 | + * | m20 m21 m22 0 | + * | 0 0 0 1 | * * Rx rotation autour de Ox d'angle O1 * Ry rotation autour de Oy d'angle O2 * Rz rotation autour de Oz d'angle O3 - * et m00 = cy.cz m01 = cy.sz m02 = -sy - * m10 = sx.sy.cz-cx.sz m11 = sx.sy.sz+cx.cz m12 = sx.cy - * m20 = cx.sy.cz+sx.sz m21 = cx.sy.sz-sx.cz m22 = cx.cy - * avec ci = cos Oi et si = sin Oi. + * et m00 = cy.cz m01 = cy.sz m02 = -sy + * m10 = sx.sy.cz-cx.sz m11 = sx.sy.sz+cx.cz m12 = sx.cy + * m20 = cx.sy.cz+sx.sz m21 = cx.sy.sz-sx.cz m22 = cx.cy + * avec ci = cos Oi et si = sin Oi. * * Entree : - * vp Rotations par rapport aux axes d'un repere droit en degres. - * m Matrice a initialiser. + * vp Rotations par rapport aux axes d'un repere droit en degres. + * m Matrice a initialiser. */ void Rotate_to_Matrix(Vector *vp, Matrix m) { - float rx = vp->x * (float)M_PI / (float)180.0, /* passage en radians */ + float rx = vp->x * (float)M_PI / (float)180.0, /* passage en radians */ ry = vp->y * (float)M_PI / (float)180.0, rz = vp->z * (float)M_PI / (float)180.0; float cx = (float)cos((double)rx), sx = (float)sin((double)rx), cy = (float)cos((double)ry), sy = (float)sin((double)ry), cz = (float)cos((double)rz), sz = (float)sin((double)rz); @@ -644,40 +644,40 @@ void Rotate_to_Matrix(Vector *vp, Matrix m) * d'angle et d'axe donnes. * Si M est la matrice, O l'angle et N le vecteur directeur de l'axe : * - * M = cos(O) Id3 + (1 - cosO) Nt N + sinO N~ + * M = cos(O) Id3 + (1 - cosO) Nt N + sinO N~ * - * | NxNxverO+ cosO NxNyverO+NzsinO NxNzverO-NxsinO 0 | - * M = | NxNyverO-NzsinO NyNyverO+ cosO NyNzverO+NxsinO 0 | - * | NxNzverO+NysinO NyNzverO-NxsinO NzNzverO+ cosO 0 | - * | 0 0 0 1 | + * | NxNxverO+ cosO NxNyverO+NzsinO NxNzverO-NxsinO 0 | + * M = | NxNyverO-NzsinO NyNyverO+ cosO NyNzverO+NxsinO 0 | + * | NxNzverO+NysinO NyNzverO-NxsinO NzNzverO+ cosO 0 | + * | 0 0 0 1 | * - * O angle de rotation. - * N Vecteur directeur norme de l'axe de rotation. - * Nt Vecteur transpose. - * N~ | 0 Nz -Ny| - * |-Nz 0 Nx| - * | Ny -Nx 0 | + * O angle de rotation. + * N Vecteur directeur norme de l'axe de rotation. + * Nt Vecteur transpose. + * N~ | 0 Nz -Ny| + * |-Nz 0 Nx| + * | Ny -Nx 0 | * Entree : - * a Angle de rotation en degres. - * axis Vecteur directeur de l'axe de la rotation. - * m Matrice a initialiser. + * a Angle de rotation en degres. + * axis Vecteur directeur de l'axe de la rotation. + * m Matrice a initialiser. */ void Rotaxis_to_Matrix(float a, Vector *axis, Matrix m) { float cosa; float sina; - float vera; /* 1 - cosa */ + float vera; /* 1 - cosa */ Vector n; /* vecteur norme*/ - Vector conv; /* verO n */ - Vector tilde; /* sinO n */ + Vector conv; /* verO n */ + Vector tilde; /* sinO n */ - a *= (float)M_PI / (float)180.0; /* passage en radians */ + a *= (float)M_PI / (float)180.0; /* passage en radians */ cosa = (float)cos((double)a); sina = (float)sin((double)a); vera = (float)1.0 - cosa; - n = *axis; /* norme le vecteur directeur */ + n = *axis; /* norme le vecteur directeur */ norm_vector(&n); tilde = conv = n; MUL_COORD3(conv, vera, vera, vera); @@ -704,14 +704,14 @@ void Rotaxis_to_Matrix(float a, Vector *axis, Matrix m) * La procedure "Rotrans_to_Matrix" initialise la matrice par la rotation * et de la translation. * Entree : - * rp Vecteur des angles de rotation en degres. - * tp Vecteur des coordonnees de translation. - * m Matrice a initialiser. + * rp Vecteur des angles de rotation en degres. + * tp Vecteur des coordonnees de translation. + * m Matrice a initialiser. */ void Rotrans_to_Matrix(Vector *rp, Vector *tp, Matrix m) { - Rotate_to_Matrix(rp, m); /* matrice de rotation */ - m[3][0] = tp->x; /* matrice de translation */ + Rotate_to_Matrix(rp, m); /* matrice de rotation */ + m[3][0] = tp->x; /* matrice de translation */ m[3][1] = tp->y; m[3][2] = tp->z; } @@ -719,8 +719,8 @@ void Rotrans_to_Matrix(Vector *rp, Vector *tp, Matrix m) /* * La procedure "Scale_to_Matrix" initialise la matrice par l'homothetie. * Entree : - * vp Vecteur des coordonnees d'homothetie. - * m Matrice a initialiser. + * vp Vecteur des coordonnees d'homothetie. + * m Matrice a initialiser. */ void Scale_to_Matrix(Vector *vp, Matrix m) { @@ -732,8 +732,8 @@ void Scale_to_Matrix(Vector *vp, Matrix m) /* * La procedure "Translate_to_Matrix" initialise la matrice par la - * translation. Entree : vp Vecteur des coordonnees de - * translation. m Matrice a initialiser. + * translation. Entree : vp Vecteur des coordonnees de + * translation. m Matrice a initialiser. */ void Translate_to_Matrix(Vector *vp, Matrix m) { diff --git a/modules/robot/src/wireframe-simulator/vpArit.h b/modules/robot/src/wireframe-simulator/vpArit.h index 3599e796f3..2e043502ad 100644 --- a/modules/robot/src/wireframe-simulator/vpArit.h +++ b/modules/robot/src/wireframe-simulator/vpArit.h @@ -175,20 +175,20 @@ typedef struct { } /* - * POSITION - * ________ + * POSITION + * ________ * * La structure "Position" definit le positionnement d'un objet. * Matrice de positionnement = R.S.T - * avec R = Rx.Ry.Rz Matrice de rotation autour des axes (Ox,Oy,Oz), - * Les angles sont donnes en degres; - * S = Sx.Sy.Sz Matrice d'homothetie sur les axes; - * T = Tx.Ty.Tz Matrice de translation sur les axes. + * avec R = Rx.Ry.Rz Matrice de rotation autour des axes (Ox,Oy,Oz), + * Les angles sont donnes en degres; + * S = Sx.Sy.Sz Matrice d'homothetie sur les axes; + * T = Tx.Ty.Tz Matrice de translation sur les axes. */ typedef struct { - Vector rotate; /* vecteur rotation */ - Vector scale; /* vecteur homothetie */ - Vector translate; /* vecteur translation */ + Vector rotate; /* vecteur rotation */ + Vector scale; /* vecteur homothetie */ + Vector translate; /* vecteur translation */ } AritPosition; #define IDENTITY_ROTATE \ diff --git a/modules/robot/src/wireframe-simulator/vpAritio.cpp b/modules/robot/src/wireframe-simulator/vpAritio.cpp index 5639cda51c..d5cd1d50bf 100644 --- a/modules/robot/src/wireframe-simulator/vpAritio.cpp +++ b/modules/robot/src/wireframe-simulator/vpAritio.cpp @@ -30,10 +30,10 @@ * * Description: * Le module "aritio.c" contient les procedures d'entree/sortie - * des types definis dans le module "arit.h". - * Les entrees non specifiees sont effectuees - * sur le fichier "source" du module "lex.c". - * Pour les mots cles des "fprintf_..." voir "token.c". + * des types definis dans le module "arit.h". + * Les entrees non specifiees sont effectuees + * sur le fichier "source" du module "lex.c". + * Pour les mots cles des "fprintf_..." voir "token.c". * * Authors: * Jean-Luc CORRE @@ -52,8 +52,8 @@ /* * La procedure "fprintf_Position" ecrit en ascii un positionnement. * Entree : - * f Fichier en sortie. - * pp Positionnement a ecrite. + * f Fichier en sortie. + * pp Positionnement a ecrite. */ void fprintf_Position(FILE *f, AritPosition *pp) { @@ -64,26 +64,26 @@ void fprintf_Position(FILE *f, AritPosition *pp) /* * La procedure "fscanf_Point3f" lit en ascii un point flottant 3D. * Entree : - * pp Point flottant 3D a lire. + * pp Point flottant 3D a lire. */ void fscanf_Point3f(Point3f *pp) { static const char *err_tbl[] = {"float expected (coordinate ", " of point)"}; int t; - /* Lecture de la premiere coordonnee du point. */ + /* Lecture de la premiere coordonnee du point. */ if ((t = lex()) != T_FLOAT && t != T_INT) lexerr("start", err_tbl[0], "X", err_tbl[1], NULL); pp->x = (t == T_INT) ? (float)myint : myfloat; - /* Lecture de la seconde coordonnee du point. */ + /* Lecture de la seconde coordonnee du point. */ if ((t = lex()) != T_FLOAT && t != T_INT) lexerr("start", err_tbl[0], "Y", err_tbl[1], NULL); pp->y = (t == T_INT) ? (float)myint : myfloat; - /* Lecture de la troisieme coordonnee du point. */ + /* Lecture de la troisieme coordonnee du point. */ if ((t = lex()) != T_FLOAT && t != T_INT) lexerr("start", err_tbl[0], "Z", err_tbl[1], NULL); @@ -93,7 +93,7 @@ void fscanf_Point3f(Point3f *pp) /* * La procedure "fscanf_Vector" lit en ascii un vecteur. * Entree : - * vp Vecteur a lire. + * vp Vecteur a lire. */ void fscanf_Vector(Vector *vp) { @@ -101,19 +101,19 @@ void fscanf_Vector(Vector *vp) int t; - /* Lecture de la premiere coordonnee du vecteur. */ + /* Lecture de la premiere coordonnee du vecteur. */ if ((t = lex()) != T_FLOAT && t != T_INT) lexerr("start", err_tbl[0], "X", err_tbl[1], NULL); vp->x = (t == T_INT) ? (float)myint : myfloat; - /* Lecture de la seconde coordonnee du vecteur. */ + /* Lecture de la seconde coordonnee du vecteur. */ if ((t = lex()) != T_FLOAT && t != T_INT) lexerr("start", err_tbl[0], "Y", err_tbl[1], NULL); vp->y = (t == T_INT) ? (float)myint : myfloat; - /* Lecture de la troisieme coordonnee du vecteur. */ + /* Lecture de la troisieme coordonnee du vecteur. */ if ((t = lex()) != T_FLOAT && t != T_INT) lexerr("start", err_tbl[0], "Z", err_tbl[1], NULL); @@ -123,7 +123,7 @@ void fscanf_Vector(Vector *vp) /* * La procedure "fscanf_Position" lit en ascii un positionnement. * Entree : - * pp Positionnement a lire. + * pp Positionnement a lire. */ void fscanf_Position(AritPosition *pp) { diff --git a/modules/robot/src/wireframe-simulator/vpAritio.h b/modules/robot/src/wireframe-simulator/vpAritio.h index 8659a39b08..847e2f5351 100644 --- a/modules/robot/src/wireframe-simulator/vpAritio.h +++ b/modules/robot/src/wireframe-simulator/vpAritio.h @@ -30,10 +30,10 @@ * * Description: * Le module "aritio.c" contient les procedures d'entree/sortie - * des types definis dans le module "arit.h". - * Les entrees non specifiees sont effectuees - * sur le fichier "source" du module "lex.c". - * Pour les mots cles des "fprintf_..." voir "token.c". + * des types definis dans le module "arit.h". + * Les entrees non specifiees sont effectuees + * sur le fichier "source" du module "lex.c". + * Pour les mots cles des "fprintf_..." voir "token.c". * * Authors: * Jean-Luc CORRE diff --git a/modules/robot/src/wireframe-simulator/vpBound.cpp b/modules/robot/src/wireframe-simulator/vpBound.cpp index 57719eb7c1..b02f2358e1 100644 --- a/modules/robot/src/wireframe-simulator/vpBound.cpp +++ b/modules/robot/src/wireframe-simulator/vpBound.cpp @@ -54,27 +54,27 @@ * La procedure "free_Bound" libere la memoire d'une surface. * Les champs "bound.face.edge" ne sont pas utilises. * Entree : - * bp Surface a liberer. + * bp Surface a liberer. */ void free_Bound(Bound *bp) { Face *fp = bp->face.ptr; Face *fend = fp + bp->face.nbr; - for (; fp < fend; fp++) { /* libere les polygones */ + for (; fp < fend; fp++) { /* libere les polygones */ if (fp->vertex.ptr != fp->vertex.tbl) free((char *)fp->vertex.ptr); } - if (bp->face.ptr != NULL) { /* libere les faces */ + if (bp->face.ptr != NULL) { /* libere les faces */ free((char *)bp->face.ptr); bp->face.ptr = NULL; } - if (bp->point.ptr != NULL) { /* libere les points */ + if (bp->point.ptr != NULL) { /* libere les points */ free((char *)bp->point.ptr); bp->point.ptr = NULL; } #ifdef face_normal - if (bp->normal.ptr != NULL) { /* libere les vecteurs */ + if (bp->normal.ptr != NULL) { /* libere les vecteurs */ free((char *)bp->normal.ptr); bp->normal.ptr = NULL; } @@ -86,32 +86,32 @@ void free_Bound(Bound *bp) * La procedure "free_huge_Bound" libere une surface de taille maximale. * La particularite de cette surface est le tableau unique des sommets. * Entree : - * bp Surface a liberer. + * bp Surface a liberer. */ void free_huge_Bound(Bound *bp) { - bp->face.nbr = 1; /* pour la liberation en une fois */ + bp->face.nbr = 1; /* pour la liberation en une fois */ free_Bound(bp); } /* * La procedure "free_Bound_scene" libere une scene de surfaces. * Entree : - * bsp Scene a liberer. + * bsp Scene a liberer. */ void free_Bound_scene(Bound_scene *bsp) { Bound *bp = bsp->bound.ptr; Bound *bend = bp + bsp->bound.nbr; - for (; bp < bend; bp++) { /* libere les surfaces */ + for (; bp < bend; bp++) { /* libere les surfaces */ free_Bound(bp); } - if (bsp->name != NULL) { /* libere le nom */ + if (bsp->name != NULL) { /* libere le nom */ free((char *)bsp->name); bsp->name = NULL; } - if (bsp->bound.ptr != NULL) { /* libere le tableau */ + if (bsp->bound.ptr != NULL) { /* libere le tableau */ free((char *)bsp->bound.ptr); bsp->bound.ptr = NULL; } @@ -121,24 +121,24 @@ void free_Bound_scene(Bound_scene *bsp) * La procedure "malloc_Bound" alloue une surface. * Les champs "bound.face.edge" ne sont pas utilises. * Entree : - * bp Surface a allouer. - * type Type de la surface. - * polygonal Booleen indiquant si la surface est polygonale. - * fn Nombre de faces de la surface. - * pn Nombre de points de la surface. + * bp Surface a allouer. + * type Type de la surface. + * polygonal Booleen indiquant si la surface est polygonale. + * fn Nombre de faces de la surface. + * pn Nombre de points de la surface. */ void malloc_Bound(Bound *bp, Type type, int polygonal, Index fn, Index pn) { static char proc_name[] = "malloc_Bound"; - if ((bp->face.nbr = fn) == 0) /* faces */ + if ((bp->face.nbr = fn) == 0) /* faces */ bp->face.ptr = NULL; else if ((bp->face.ptr = (Face *)malloc(fn * sizeof(Face))) == NULL) { perror(proc_name); throw vpException(vpException::fatalError, "Error in malloc_Bound"); } - if ((bp->point.nbr = pn) == 0) /* points */ + if ((bp->point.nbr = pn) == 0) /* points */ bp->point.ptr = NULL; else if ((bp->point.ptr = (Point3f *)malloc(pn * sizeof(Point3f))) == NULL) { perror(proc_name); @@ -146,7 +146,7 @@ void malloc_Bound(Bound *bp, Type type, int polygonal, Index fn, Index pn) } #ifdef face_normal - /* normales aux sommets */ + /* normales aux sommets */ if ((bp->normal.nbr = (bp->is_polygonal ? 0 : pn)) == 0) bp->normal.ptr = NULL; else if ((bp->normal.ptr = (Vector *)malloc(pn * sizeof(Vector))) == NULL) { @@ -165,16 +165,16 @@ void malloc_Bound(Bound *bp, Type type, int polygonal, Index fn, Index pn) * La surface est adaptee pour la reception de tout type de surface. * La surface allouee peut etre utilisee comme une surface de travail. * Sa taille est definie par les macros "..._NBR" de "world.h". - * FACE_NBR : son nombre de faces - * POINT_NBR : son nombre de points - * VECTOR_NBR : son monbre de vecteurs - * VERTEX_NBR : son nombre de sommets par face. + * FACE_NBR : son nombre de faces + * POINT_NBR : son nombre de points + * VECTOR_NBR : son monbre de vecteurs + * VERTEX_NBR : son nombre de sommets par face. * La particularite de la surface vient de l'allocation en une seule fois * d'un tableau de sommets. Les polygones des faces ne sont pas initialiser, * exepte celui de la premiere face qui est la base du tableau des sommets. * Les champs "bound.face.edge" ne sont pas utilises. * Entree : - * bp Surface maximale a allouer. + * bp Surface maximale a allouer. */ void malloc_huge_Bound(Bound *bp) { @@ -196,9 +196,9 @@ void malloc_huge_Bound(Bound *bp) * Stocke le nom de la scene et alloue l'espace memoire necessaire. * Les champs "bound.face.edge" ne sont pas utilises. * Entree : - * bsp Scene a allouer. - * name Nom de la scene. - * bn Nombre de surfaces de la scene. + * bsp Scene a allouer. + * name Nom de la scene. + * bn Nombre de surfaces de la scene. */ void malloc_Bound_scene(Bound_scene *bsp, const char *name, Index bn) { diff --git a/modules/robot/src/wireframe-simulator/vpBound.h b/modules/robot/src/wireframe-simulator/vpBound.h index 7870aef838..10ead99c45 100644 --- a/modules/robot/src/wireframe-simulator/vpBound.h +++ b/modules/robot/src/wireframe-simulator/vpBound.h @@ -71,7 +71,7 @@ #define BND_NBR 12 #define BOUND_NBR 1024 -#define FACE_NBR 6144 /* Tailles de tableaux */ +#define FACE_NBR 6144 /* Tailles de tableaux */ #define VERTEX_NBR 16 #define POINT_NBR 6144 #ifdef face_normal @@ -80,34 +80,34 @@ #ifdef face_edge typedef struct { - Index v0, v1; /* extremites */ - Index f0, f1; /* faces */ + Index v0, v1; /* extremites */ + Index f0, f1; /* faces */ } Edge; #endif // face_edge #ifdef face_edge typedef struct { - Index nbr; /* nombre d'aretes */ - Edge *ptr; /* liste dynamique */ + Index nbr; /* nombre d'aretes */ + Edge *ptr; /* liste dynamique */ } Edge_list; #endif // face_edge #ifdef face_normal typedef struct { - Index nbr; /* nombre de vecteurs */ - Vector *ptr; /* liste dynamique */ + Index nbr; /* nombre de vecteurs */ + Vector *ptr; /* liste dynamique */ } Vector_list; #endif // face_normal typedef struct { - float xmin, xmax; /* bornes sur l'axe x */ - float ymin, ymax; /* bornes sur l'axe y */ - float zmin, zmax; /* bornes sur l'axe z */ + float xmin, xmax; /* bornes sur l'axe x */ + float ymin, ymax; /* bornes sur l'axe y */ + float zmin, zmax; /* bornes sur l'axe z */ } Bounding_box; typedef struct { - Index nbr; /* nombre de scenes */ - Bound_scene *ptr; /* liste dynamique */ + Index nbr; /* nombre de scenes */ + Bound_scene *ptr; /* liste dynamique */ } Bound_scene_list; void free_Bound(Bound *bp); diff --git a/modules/robot/src/wireframe-simulator/vpBoundio.cpp b/modules/robot/src/wireframe-simulator/vpBoundio.cpp index e267edb185..885dc77f69 100644 --- a/modules/robot/src/wireframe-simulator/vpBoundio.cpp +++ b/modules/robot/src/wireframe-simulator/vpBoundio.cpp @@ -58,25 +58,25 @@ /* * La procedure "fscanf_Bound" lit en ascii une surface. * Entree : - * bp Surface a lire. + * bp Surface a lire. */ void fscanf_Bound(Bound *bp) { - /* Lecture du type polygonale de la surface. */ + /* Lecture du type polygonale de la surface. */ skip_keyword(T_TYPE, "bound: keyword \"type\" expected"); if (lex() != T_INT) lexerr("start", "bound_type: boolean expected (0=FALSE|~0=TRUE)", NULL); bp->is_polygonal = (myint ? 1 : 0); - /* Lecture de la liste de points de la surface. */ + /* Lecture de la liste de points de la surface. */ skip_keyword(T_POINT_LIST, "bound: keyword \"point_list\" expected"); pusherr("bound_point_list: "); fscanf_Point3f_list(&bp->point); poperr(); - /* Lecture de la liste de faces de la surface. */ + /* Lecture de la liste de faces de la surface. */ skip_keyword(T_FACE_LIST, "bound: keyword \"face_list\" expected"); pusherr("bound_face_list: "); @@ -87,20 +87,20 @@ void fscanf_Bound(Bound *bp) /* * La procedure "fscanf_Face_list" lit en ascii une liste de faces. * Entree : - * lp Liste de faces a lire. + * lp Liste de faces a lire. */ void fscanf_Face_list(Face_list *lp) { - Face *fp; /* face courante */ - Face *fend; /* borne de "fp" */ + Face *fp; /* face courante */ + Face *fend; /* borne de "fp" */ - /* Lecture du nombre de faces de la liste */ + /* Lecture du nombre de faces de la liste */ if (lex() != T_INT) lexerr("start", "integer expected (number of faces)", NULL); lp->nbr = (Index)myint; - /* Allocation dynamique de la liste de faces. */ + /* Allocation dynamique de la liste de faces. */ if (lp->nbr == 0) lp->ptr = NULL; @@ -110,28 +110,28 @@ void fscanf_Face_list(Face_list *lp) throw vpException(vpException::fatalError, "Error in fscanf_Face_list"); } - /* Lecture des faces de la liste une a une. */ + /* Lecture des faces de la liste une a une. */ fp = lp->ptr; fend = fp + lp->nbr; for (; fp < fend; fp++) { Vertex_list *vlp = &fp->vertex; - Index *vp; /* sommet courant */ - Index *vend; /* borne de "vp" */ + Index *vp; /* sommet courant */ + Index *vend; /* borne de "vp" */ - /* Lecture du type polygonale de la face. */ + /* Lecture du type polygonale de la face. */ if (lex() != T_INT) lexerr("start", "boolean expected (0=FALSE|~0=TRUE)", NULL); fp->is_polygonal = (myint ? 1 : 0); - /* Lecture du nombre de sommets de la face. */ + /* Lecture du nombre de sommets de la face. */ if (lex() != T_INT) lexerr("start", "integer expected (number of vertices)", NULL); vlp->nbr = (Index)myint; - /* Allocation dynamique du polygone de la face. */ + /* Allocation dynamique du polygone de la face. */ if (vlp->nbr <= DEFAULT_VSIZE) vlp->ptr = vlp->tbl; @@ -141,7 +141,7 @@ void fscanf_Face_list(Face_list *lp) throw vpException(vpException::fatalError, "Error in fscanf_Face_list"); } - /* Lecture des sommets de la face un a un. */ + /* Lecture des sommets de la face un a un. */ vp = vlp->ptr; vend = vp + vlp->nbr; @@ -154,21 +154,21 @@ void fscanf_Face_list(Face_list *lp) /* * La procedure "fscanf_Point_list" lit en ascii une liste de points 3D. * Entree : - * lp Liste de points a lire. + * lp Liste de points a lire. */ void fscanf_Point3f_list(Point3f_list *lp) { static const char *err_tbl[] = {"float expected (coordinate ", " of point)"}; - Point3f *pp; /* point courant */ - Point3f *pend; /* borne de "pp" */ + Point3f *pp; /* point courant */ + Point3f *pend; /* borne de "pp" */ - /* Lecture du nombre de points de la liste. */ + /* Lecture du nombre de points de la liste. */ if (lex() != T_INT) lexerr("start", "integer expected (number of points 3D)", NULL); lp->nbr = (Index)myint; /* FC printf("nbr %d\n",lp->nbr); */ - /* Allocation dynamique la liste de points. */ + /* Allocation dynamique la liste de points. */ if (lp->nbr == 0) lp->ptr = NULL; @@ -178,7 +178,7 @@ void fscanf_Point3f_list(Point3f_list *lp) throw vpException(vpException::fatalError, "Error in fscanf_Point3f_list"); } - /* Lecture des points de la liste un a un. */ + /* Lecture des points de la liste un a un. */ pp = lp->ptr; pend = pp + lp->nbr; diff --git a/modules/robot/src/wireframe-simulator/vpClipping.cpp b/modules/robot/src/wireframe-simulator/vpClipping.cpp index f5bd7b72ca..e4fa204653 100644 --- a/modules/robot/src/wireframe-simulator/vpClipping.cpp +++ b/modules/robot/src/wireframe-simulator/vpClipping.cpp @@ -60,21 +60,21 @@ static void point_4D_3D(Point4f *p4, int size, Byte *cp, Point3f *p3); /* * Variables utilisees par le decoupage : * - * CLIP : + * CLIP : * Surface resultat apres le decoupage. * La surface est adaptee pour la reception de tous les types de surfaces. * Sa taille est definie par les macros "..._NBR" de "bound.h". * - * FACE_NBR : son nombre de faces - * POINT_NBR : son nombre de points - * VECTOR_NBR : son monbre de vecteurs - * VERTEX_NBR : son nombre de sommets par face. + * FACE_NBR : son nombre de faces + * POINT_NBR : son nombre de points + * VECTOR_NBR : son monbre de vecteurs + * VERTEX_NBR : son nombre de sommets par face. * * La surface recoit une a une les faces decoupees. * La surface recoit en une fois tous les points 3D de la surface decoupee * par rapport aux 6 plans de la pyramide tronquee de vision. * - * CODE : + * CODE : * Tableau de booleens durant le decoupage. * Le tableau est initialise par les booleens indiquant le positionnement des * points 4D par rapport aux 6 plans de decoupage. @@ -88,16 +88,16 @@ static void point_4D_3D(Point4f *p4, int size, Byte *cp, Point3f *p3); * Le tableau recoit ensuite un a un les points 4D d'intersection de la * surface avec les 6 plans de la pyramide tronquee de vision. */ -static Bound clip; /* surface a decouper */ -static Byte *code; /* tableau de bits */ -static Point4f *point4f; /* tableau de points 4D */ -static Index point4f_nbr; /* nombre de points 4D */ +static Bound clip; /* surface a decouper */ +static Byte *code; /* tableau de bits */ +static Point4f *point4f; /* tableau de points 4D */ +static Index point4f_nbr; /* nombre de points 4D */ #if clip_opt static Index *poly0, *poly1; /* polygones temporaires*/ #else -static Index *poly_tmp; /* polygone temporaire */ -#endif /* clip_opt */ +static Index *poly_tmp; /* polygone temporaire */ +#endif /* clip_opt */ /* * La procedure "open_clipping" alloue et initialise les variables utilisees @@ -105,10 +105,10 @@ static Index *poly_tmp; /* polygone temporaire */ */ void open_clipping(void) { - /* alloue la surface de travail */ + /* alloue la surface de travail */ malloc_huge_Bound(&clip); - /* alloue les tableaux */ + /* alloue les tableaux */ if ((code = (Byte *)malloc(POINT_NBR * sizeof(Byte))) == NULL || (point4f = (Point4f *)malloc(POINT_NBR * sizeof(Point4f))) == NULL #ifdef clip_opt @@ -124,7 +124,7 @@ void open_clipping(void) perror(proc_name); throw vpException(vpException::fatalError, "Error in open_clipping"); } -#endif /* clip_opt */ +#endif /* clip_opt */ } /* @@ -141,51 +141,51 @@ void close_clipping(void) free((char *)poly1); #else free((char *)poly_tmp); -#endif /* clip_opt */ +#endif /* clip_opt */ } /* * La procedure "clipping" decoupe un polygone par rapport a un plan * suivant l'algorithme de Sutherland et Hodgman. * Entree : - * mask Masque du plan de decoupage pour le code. - * vni Nombre de sommets du polygone en entree. - * pi Polygone en entree. - * po Polygone resultat du decoupage. + * mask Masque du plan de decoupage pour le code. + * vni Nombre de sommets du polygone en entree. + * pi Polygone en entree. + * po Polygone resultat du decoupage. * Sortie : - * Nombre de sommets du polygone resultat "po". + * Nombre de sommets du polygone resultat "po". */ static Index clipping(Byte mask, Index vni, Index *pi, Index *po) { /* - * vno Nombre de sommets du polygone "po". - * vs Premier sommet de l'arete a decouper. - * vp Second sommet de l'arete a decouper. - * ins TRUE si le sommet "vs" est interieur, FALSE sinon. - * inp TRUE si le sommet "vp" est interieur, FALSE sinon. + * vno Nombre de sommets du polygone "po". + * vs Premier sommet de l'arete a decouper. + * vp Second sommet de l'arete a decouper. + * ins TRUE si le sommet "vs" est interieur, FALSE sinon. + * inp TRUE si le sommet "vp" est interieur, FALSE sinon. */ - Index vno = vni; /* nombre de sommets */ - Index vs = pi[vni - 1]; /* premier sommet */ - Byte ins = code[vs] & mask; /* code de "vs" */ + Index vno = vni; /* nombre de sommets */ + Index vs = pi[vni - 1]; /* premier sommet */ + Byte ins = code[vs] & mask; /* code de "vs" */ - while (vni--) { /* pour tous les sommets */ - Index vp = *pi++; /* second sommet */ - Byte inp = code[vp] & mask; /* code du plan courant */ + while (vni--) { /* pour tous les sommets */ + Index vp = *pi++; /* second sommet */ + Byte inp = code[vp] & mask; /* code du plan courant */ if (ins == IS_INSIDE) { - if (inp == IS_INSIDE) { /* arete interieure */ + if (inp == IS_INSIDE) { /* arete interieure */ *po++ = vp; - } else { /* intersection */ + } else { /* intersection */ inter(mask, vs, vp); *po++ = point4f_nbr++; } } else { - if (inp == IS_INSIDE) { /* intersection */ + if (inp == IS_INSIDE) { /* intersection */ inter(mask, vs, vp); *po++ = point4f_nbr++; *po++ = vp; vno++; - } else { /* arete exterieure */ + } else { /* arete exterieure */ vno--; } } @@ -199,16 +199,16 @@ static Index clipping(Byte mask, Index vni, Index *pi, Index *po) * La procedure "clipping_Face" decoupe une face par rapport aux 6 plans * de decoupage de la pyramide tronquee de vision. * Entree : - * fi Face a decouper. - * fo Face resultat du decoupage. + * fi Face a decouper. + * fo Face resultat du decoupage. * Sortie : - * Le nombre de sommets de la face resultat. + * Le nombre de sommets de la face resultat. */ static Index clipping_Face(Face *fi, Face *fo) { - Index *flip = poly_tmp; /* polygone temporaire */ - Index *flop = fo->vertex.ptr; /* polygone resultat */ - Index vn = fi->vertex.nbr; /* nombre de sommets */ + Index *flip = poly_tmp; /* polygone temporaire */ + Index *flop = fo->vertex.ptr; /* polygone resultat */ + Index vn = fi->vertex.nbr; /* nombre de sommets */ if ((vn = clipping(IS_ABOVE, vn, fi->vertex.ptr, flip)) != 0) if ((vn = clipping(IS_BELOW, vn, flip, flop)) != 0) @@ -216,14 +216,14 @@ static Index clipping_Face(Face *fi, Face *fo) if ((vn = clipping(IS_LEFT, vn, flip, flop)) != 0) if ((vn = clipping(IS_BACK, vn, flop, flip)) != 0) if ((vn = clipping(IS_FRONT, vn, flip, flop)) != 0) { - /* recopie de "fi" dans "fo" */ - /* fo->vertex.ptr == flop */ + /* recopie de "fi" dans "fo" */ + /* fo->vertex.ptr == flop */ fo->vertex.nbr = vn; fo->is_polygonal = fi->is_polygonal; fo->is_visible = fi->is_visible; #ifdef face_normal fo->normal = fi->normal; -#endif /* face_normal */ +#endif /* face_normal */ return (vn); } return (0); @@ -235,19 +235,19 @@ static Index clipping_Face(Face *fi, Face *fo) * Les calculs geometriques sont effectues en coordonnees homogenes. * Note : Les points invisibles de la surface "clip" ont une profondeur *negative c'est a dire une coordonnee Z negative. Entree : - * bp Surface a decouper. - * m Matrice de projection dans le volume canonique. + * bp Surface a decouper. + * m Matrice de projection dans le volume canonique. * Sortie : - * Pointeur de la surface resultat "clip" si elle est visible, - * NULL sinon. + * Pointeur de la surface resultat "clip" si elle est visible, + * NULL sinon. */ Bound *clipping_Bound(Bound *bp, Matrix m) { - Face *fi = bp->face.ptr; /* 1ere face */ + Face *fi = bp->face.ptr; /* 1ere face */ Face *fend = fi + bp->face.nbr; /* borne de "fi"*/ - Face *fo = clip.face.ptr; /* face clippee */ + Face *fo = clip.face.ptr; /* face clippee */ - /* recopie de "bp" dans les tableaux intermediaires */ + /* recopie de "bp" dans les tableaux intermediaires */ point4f_nbr = bp->point.nbr; point_3D_4D(bp->point.ptr, (int)point4f_nbr, m, point4f); @@ -255,12 +255,12 @@ Bound *clipping_Bound(Bound *bp, Matrix m) #ifdef face_normal if (!(clip.is_polygonal = bp->is_polygonal)) // bcopy (bp->normal.ptr, clip.normal.ptr, - // bp->normal.nbr * sizeof (Vector)); + // bp->normal.nbr * sizeof (Vector)); memmove(clip.normal.ptr, bp->normal.ptr, bp->normal.nbr * sizeof(Vector)); -#endif /* face_normal */ +#endif /* face_normal */ for (; fi < fend; fi++) { /* pour toutes les faces*/ if (clipping_Face(fi, fo) != 0) { - fo++; /* ajoute la face a "clip" */ + fo++; /* ajoute la face a "clip" */ /* * Construction a la volee du future polygone. * dont l'espace memoire est deja alloue (voir @@ -271,9 +271,9 @@ Bound *clipping_Bound(Bound *bp, Matrix m) } if (fo == clip.face.ptr) - return (NULL); /* Rien a voir, circulez... */ + return (NULL); /* Rien a voir, circulez... */ - /* recopie des tableaux intermediaires dans "clip" */ + /* recopie des tableaux intermediaires dans "clip" */ point_4D_3D(point4f, (int)point4f_nbr, code, clip.point.ptr); clip.type = bp->type; @@ -282,7 +282,7 @@ Bound *clipping_Bound(Bound *bp, Matrix m) #ifdef face_normal if (!bp->is_polygonal) clip.normal.nbr = point4f_nbr; -#endif /* face_normal */ +#endif /* face_normal */ return (&clip); } @@ -290,81 +290,81 @@ Bound *clipping_Bound(Bound *bp, Matrix m) * La procedure "inter" calcule le point d'intersection "point4f[point4f_nbr]" * de l'arete (v0,v1) avec le plan "mask". * Entree : - * mask Mask du plan de decoupage. - * v0 Permier sommet de l'arete. - * v1 Second sommet de l'arete. + * mask Mask du plan de decoupage. + * v0 Permier sommet de l'arete. + * v1 Second sommet de l'arete. */ static void inter(Byte mask, Index v0, Index v1) { Point4f *p = point4f + point4f_nbr; Point4f *p0 = point4f + v0; Point4f *p1 = point4f + v1; - float t; /* parametre entre 0 et 1 */ + float t; /* parametre entre 0 et 1 */ - /* calcule le point d'intersection */ + /* calcule le point d'intersection */ switch (mask) { case IS_ABOVE: - /* t = (p0->w - p0->y) / ((p0->w - p0->y) - (p1->w - p1->y)); */ + /* t = (p0->w - p0->y) / ((p0->w - p0->y) - (p1->w - p1->y)); */ t = (p0->w - p0->y) - (p1->w - p1->y); // t = (t == 0) ? (float)1.0 : (p0->w - p0->y) / t; t = (std::fabs(t) <= std::numeric_limits::epsilon()) ? (float)1.0 : (p0->w - p0->y) / t; PAR_COORD3(*p, t, *p0, *p1); - p->w = p->y; /* propriete du point d'intersection */ + p->w = p->y; /* propriete du point d'intersection */ break; case IS_BELOW: - /* t = (p0->w + p0->y) / ((p0->w + p0->y) - (p1->w + p1->y)); */ + /* t = (p0->w + p0->y) / ((p0->w + p0->y) - (p1->w + p1->y)); */ t = (p0->w + p0->y) - (p1->w + p1->y); // t = (t == 0) ? (float)1.0 : (p0->w + p0->y) / t; t = (std::fabs(t) <= std::numeric_limits::epsilon()) ? (float)1.0 : (p0->w + p0->y) / t; PAR_COORD3(*p, t, *p0, *p1); - p->w = -p->y; /* propriete du point d'intersection */ + p->w = -p->y; /* propriete du point d'intersection */ break; case IS_RIGHT: - /* t = (p0->w - p0->x) / ((p0->w - p0->x) - (p1->w - p1->x)); */ + /* t = (p0->w - p0->x) / ((p0->w - p0->x) - (p1->w - p1->x)); */ t = (p0->w - p0->x) - (p1->w - p1->x); // t = (t == 0) ? (float)1.0 : (p0->w - p0->x) / t; t = (std::fabs(t) <= std::numeric_limits::epsilon()) ? (float)1.0 : (p0->w - p0->x) / t; PAR_COORD3(*p, t, *p0, *p1); - p->w = p->x; /* propriete du point d'intersection */ + p->w = p->x; /* propriete du point d'intersection */ break; case IS_LEFT: - /* t = (p0->w + p0->x) / ((p0->w + p0->x) - (p1->w + p1->x)); */ + /* t = (p0->w + p0->x) / ((p0->w + p0->x) - (p1->w + p1->x)); */ t = (p0->w + p0->x) - (p1->w + p1->x); // t = (t == 0) ? (float)1.0 : (p0->w + p0->x) / t; t = (std::fabs(t) <= std::numeric_limits::epsilon()) ? (float)1.0 : (p0->w + p0->x) / t; PAR_COORD3(*p, t, *p0, *p1); - p->w = -p->x; /* propriete du point d'intersection */ + p->w = -p->x; /* propriete du point d'intersection */ break; case IS_BACK: - /* t = (p0->w - p0->z) / ((p0->w - p0->z) - (p1->w - p1->z)); */ + /* t = (p0->w - p0->z) / ((p0->w - p0->z) - (p1->w - p1->z)); */ t = (p0->w - p0->z) - (p1->w - p1->z); // t = (t == 0) ? (float)1.0 : (p0->w - p0->z) / t; t = (std::fabs(t) <= std::numeric_limits::epsilon()) ? (float)1.0 : (p0->w - p0->z) / t; PAR_COORD3(*p, t, *p0, *p1); - p->w = p->z; /* propriete du point d'intersection */ + p->w = p->z; /* propriete du point d'intersection */ break; case IS_FRONT: - /* t = p0->z / (p0->z - p1->z); */ + /* t = p0->z / (p0->z - p1->z); */ t = (p0->z - p1->z); // t = (t == 0) ? (float)1.0 : p0->z / t; t = (std::fabs(t) <= std::numeric_limits::epsilon()) ? (float)1.0 : p0->z / t; p->x = (p1->x - p0->x) * t + p0->x; p->y = (p1->y - p0->y) * t + p0->y; p->w = (p1->w - p0->w) * t + p0->w; - p->z = (float)M_EPSILON; /* propriete du point d'intersection */ + p->z = (float)M_EPSILON; /* propriete du point d'intersection */ break; } - /* resout les problemes d'arrondis pour "where_is_Point4f" */ - /* p->w += (p->w < 0) ? (- M_EPSILON) : M_EPSILON; */ + /* resout les problemes d'arrondis pour "where_is_Point4f" */ + /* p->w += (p->w < 0) ? (- M_EPSILON) : M_EPSILON; */ p->w += (float)M_EPSILON; - code[point4f_nbr] = where_is_Point4f(p); /* localise "p" */ + code[point4f_nbr] = where_is_Point4f(p); /* localise "p" */ #ifdef face_normal if (!clip.is_polygonal) { Vector *n0 = clip.normal.ptr + v0; @@ -373,32 +373,32 @@ static void inter(Byte mask, Index v0, Index v1) SET_COORD3(*n, (n1->x - n0->x) * t + n0->x, (n1->y - n0->y) * t + n0->y, (n1->z - n0->z) * t + n0->z); } -#endif /* face_normal */ +#endif /* face_normal */ } /* * La procedure "point_4D_3D" transforme les points homogenes 4D visibles * en points 3D par projection. - * Note : On marque un point 3D invisible par une profondeur negative. + * Note : On marque un point 3D invisible par une profondeur negative. * Entree : - * p4 Tableau de points 4D a transformer. - * size Taille du tableau "p4". - * cp Tableau de code indiquant la visibilite des points 4D. - * p3 Tableau de points 3D issus de la transformation. + * p4 Tableau de points 4D a transformer. + * size Taille du tableau "p4". + * cp Tableau de code indiquant la visibilite des points 4D. + * p3 Tableau de points 3D issus de la transformation. */ static void point_4D_3D(Point4f *p4, int size, Byte *cp, Point3f *p3) { - Point4f *pend = p4 + size; /* borne de p4 */ + Point4f *pend = p4 + size; /* borne de p4 */ float w; for (; p4 < pend; p4++, p3++) { - if (*cp++ == IS_INSIDE) { /* point visible */ + if (*cp++ == IS_INSIDE) { /* point visible */ w = p4->w; - p3->x = p4->x / w; /* projection 4D en 3D */ + p3->x = p4->x / w; /* projection 4D en 3D */ p3->y = p4->y / w; p3->z = p4->z / w; - } else { /* marque invisible */ + } else { /* marque invisible */ p3->z = -1.0; } } @@ -408,14 +408,14 @@ static void point_4D_3D(Point4f *p4, int size, Byte *cp, Point3f *p3) * La procedure "set_Point4f_code" initialise la position des points 4D * par rapport a 6 plans de la pyramide tronquee de vision. * A chaque point est associe un code indiquant la position respective du - * point. Entree : p4 Tableau de points 4D a localiser. size - * Taille du tableau "p4". cp Tableau de codes de localisation + * point. Entree : p4 Tableau de points 4D a localiser. size + * Taille du tableau "p4". cp Tableau de codes de localisation * resultat. */ void set_Point4f_code(Point4f *p4, int size, Byte *cp) { - Point4f *pend = p4 + size; /* borne de p4 */ - Byte b; /* code de p4 */ + Point4f *pend = p4 + size; /* borne de p4 */ + Byte b; /* code de p4 */ for (; p4 < pend; p4++, *cp++ = b) { b = IS_INSIDE; @@ -442,7 +442,7 @@ void set_Point4f_code(Point4f *p4, int size, Byte *cp) */ Byte where_is_Point4f(Point4f *p4) { - Byte b = IS_INSIDE; /* code de "p4" */ + Byte b = IS_INSIDE; /* code de "p4" */ if (p4->w < p4->y) b |= IS_ABOVE; diff --git a/modules/robot/src/wireframe-simulator/vpCoreDisplay.cpp b/modules/robot/src/wireframe-simulator/vpCoreDisplay.cpp index 947d778484..f77e4151bd 100644 --- a/modules/robot/src/wireframe-simulator/vpCoreDisplay.cpp +++ b/modules/robot/src/wireframe-simulator/vpCoreDisplay.cpp @@ -52,10 +52,10 @@ #include "vpVwstack.h" /* - * POINT2I : + * POINT2I : * Tableau de points 2D dans l'espace ecran servant a l'affichage fil-de-fer. * - * RENAME : + * RENAME : * Tableau de renommage des sommets ou tableau de compteurs associes aux * points. */ @@ -96,25 +96,25 @@ void close_display(void) * La procedure "point_3D_2D" projette les points 3D du volume canonique * dans l'espace image 2D. * - * Volume canonique Espace image - * ________________ ____________ + * Volume canonique Espace image + * ________________ ____________ * - * - 1 < X < 1 0 < X < xsize - * - 1 < Y < 1 0 < Y < ysize - * 0 < Z < 1 + * - 1 < X < 1 0 < X < xsize + * - 1 < Y < 1 0 < Y < ysize + * 0 < Z < 1 * - * Z < 0 X = 0, Y = -1 non significatifs. + * Z < 0 X = 0, Y = -1 non significatifs. * * Entree : - * p3 Tableau de points 3D a projeter. - * size Taille du tableau de points "p3". - * xsize, ysize Tailles de l'espace image. - * p2 Tableau de points 2D en sortie. + * p3 Tableau de points 3D a projeter. + * size Taille du tableau de points "p3". + * xsize, ysize Tailles de l'espace image. + * p2 Tableau de points 2D en sortie. */ // static void point_3D_2D(Point3f *p3, Index size, int xsize, int ysize, Point2i *p2) { - Point3f *pend = p3 + size; /* borne de p3 */ + Point3f *pend = p3 + size; /* borne de p3 */ float xdiv2 = ((float)xsize) / (float)2.0; float ydiv2 = ((float)ysize) / (float)2.0; @@ -130,18 +130,18 @@ void point_3D_2D(Point3f *p3, Index size, int xsize, int ysize, Point2i *p2) * Soit la face comportant le contour oriente suivant : (...,P2,P0,P1...). * La normale a la face au point P0 est obtenue par le produit vectoriel : * - * | x1 - x0 x2 - x0 | | Nx | - * N = (P1 - P0) ^ (P2 - P0) = | y1 - y0 y2 - y0 | = | Ny | - * | z1 - z0 z2 - z0 | | Nz | + * | x1 - x0 x2 - x0 | | Nx | + * N = (P1 - P0) ^ (P2 - P0) = | y1 - y0 y2 - y0 | = | Ny | + * | z1 - z0 z2 - z0 | | Nz | * * La face est dans le volume canonique de vision et dans un repere gauche. * L'observateur est situe a l'infini dans la direction [0, 0, -1]. - * IS_ABOVE <=> Ny < 0, IS_BELOW <=> Ny > 0. - * IS_RIGHT <=> Nx < 0, IS_LEFT <=> Nx > 0. - * IS_BACK <=> Nz < 0, IS_FRONT <=> Nz > 0. + * IS_ABOVE <=> Ny < 0, IS_BELOW <=> Ny > 0. + * IS_RIGHT <=> Nx < 0, IS_LEFT <=> Nx > 0. + * IS_BACK <=> Nz < 0, IS_FRONT <=> Nz > 0. * Entree : - * bp Surface a initialiser. - * b Drapeaux indiquant les faces non affichables. + * bp Surface a initialiser. + * b Drapeaux indiquant les faces non affichables. */ void set_Bound_face_display(Bound *bp, Byte b) { @@ -151,9 +151,9 @@ void set_Bound_face_display(Bound *bp, Byte b) for (; fp < fend; fp++) { Index *vp; - Point3f *p0; /* premier sommet */ - Point3f *p1; /* second sommet */ - Point3f *p2; /* dernier sommet */ + Point3f *p0; /* premier sommet */ + Point3f *p1; /* second sommet */ + Point3f *p2; /* dernier sommet */ fp->is_visible = TRUE; if (b == IS_INSIDE) @@ -199,12 +199,12 @@ void set_Bound_face_display(Bound *bp, Byte b) * Les points des sommets de la face sont contenu dans les points "pp" * de la surface contenant la face. * Entree : - * fp face a afficher. - * pp Points de la surface contenant la face. + * fp face a afficher. + * pp Points de la surface contenant la face. */ void wireframe_Face(Face *fp, Point2i *pp) { - // extern Window id_window; + // extern Window id_window; Index *vp = fp->vertex.ptr; Index *vend = vp + fp->vertex.nbr; diff --git a/modules/robot/src/wireframe-simulator/vpKeyword.cpp b/modules/robot/src/wireframe-simulator/vpKeyword.cpp index 231d3de46f..09959683d3 100644 --- a/modules/robot/src/wireframe-simulator/vpKeyword.cpp +++ b/modules/robot/src/wireframe-simulator/vpKeyword.cpp @@ -63,24 +63,24 @@ static char *get_keyword(void); #define NEXT(x) (x) = (x)->next typedef struct bucket { - struct bucket *next; /* element suivant */ - char *ident; /* identifateur */ - Byte length; /* longueur de "ident" */ - Index token; /* code du jeton */ + struct bucket *next; /* element suivant */ + char *ident; /* identifateur */ + Byte length; /* longueur de "ident" */ + Index token; /* code du jeton */ } Bucket; -static Bucket **hash_tbl; /* table de "hash-coding" */ +static Bucket **hash_tbl; /* table de "hash-coding" */ /* * La procedure "open_keyword" alloue et initialise les variables utilisees * par les procedures de gestion des mots cles. * Entree : - * kwp Tableau des mots cles termine par NULL. + * kwp Tableau des mots cles termine par NULL. */ void open_keyword(Keyword *kwp) { open_hash(); - for (; kwp->ident != NULL; kwp++) /* recopie les mots cles */ + for (; kwp->ident != NULL; kwp++) /* recopie les mots cles */ insert_keyword(kwp->ident, kwp->token); } @@ -115,33 +115,33 @@ static void close_hash(void) { Bucket **head = hash_tbl; Bucket **bend = head + PRIME; - Bucket *bp; /* element courant */ - Bucket *next; /* element suivant */ + Bucket *bp; /* element courant */ + Bucket *next; /* element suivant */ - for (; head < bend; head++) { /* libere les listes */ + for (; head < bend; head++) { /* libere les listes */ for (bp = *head; bp != NULL; bp = next) { next = bp->next; free((char *)bp); } } - free((char *)hash_tbl); /* libere la table */ + free((char *)hash_tbl); /* libere la table */ } /* * La procedure "hashpjw" calcule un indice code a partir de la chaine * de caracteres "str". * Pour plus de renseignements, voir : - * "Compilers. Principles, Techniques, and Tools", - * A.V. AHO, R. SETHI, J.D. ULLMAN, - * ADDISON-WESLEY PUBLISHING COMPANY, pp 436. + * "Compilers. Principles, Techniques, and Tools", + * A.V. AHO, R. SETHI, J.D. ULLMAN, + * ADDISON-WESLEY PUBLISHING COMPANY, pp 436. * Entree : - * str Chaine de caracteres a coder. + * str Chaine de caracteres a coder. * Sortie : - * Le code de la chaine. + * Le code de la chaine. */ static int hashpjw(const char *str) { - unsigned h = 0; /* "hash value" */ + unsigned h = 0; /* "hash value" */ for (; *str != '\0'; str++) { unsigned g; @@ -159,8 +159,8 @@ static int hashpjw(const char *str) * de la table de "hachage" le mot cle ayant pour identificateur * la chaine de caracteres "str" et pour valeur "token". * Entree : - * str Chaine de caracteres du mot cle. - * token Valeur du jeton associe au mot cle. + * str Chaine de caracteres du mot cle. + * token Valeur du jeton associe au mot cle. */ static void insert_keyword(const char *str, Index token) { @@ -179,7 +179,7 @@ static void insert_keyword(const char *str, Index token) bp->ident = (char *)(bp + 1); strcpy(bp->ident, str); - bp->next = *head; /* insere "b" en tete de "head" */ + bp->next = *head; /* insere "b" en tete de "head" */ *head = bp; } @@ -187,12 +187,12 @@ static void insert_keyword(const char *str, Index token) * La pocedure "get_symbol" verifie que la chaine pointee par "ident" * de longeur "length" est un mot cle. * Entree : - * ident Chaine de l'identificateur. - * length Nombre de caracteres de la chaine. + * ident Chaine de l'identificateur. + * length Nombre de caracteres de la chaine. * Note : * La chaine "ident" n'est pas terminee par '\0'. * Sortie : - * Valeur du jeton associe si c'est un mot cle, 0 sinon. + * Valeur du jeton associe si c'est un mot cle, 0 sinon. */ Index get_symbol(char *ident, int length) { @@ -201,8 +201,8 @@ Index get_symbol(char *ident, int length) char *idn = ident; int len = length; - { /* calcule le code de hachage (voir "hashpjw") */ - unsigned h = 0; /* "hash value" */ + { /* calcule le code de hachage (voir "hashpjw") */ + unsigned h = 0; /* "hash value" */ for (; len != 0; idn++, len--) { unsigned g; @@ -215,7 +215,7 @@ Index get_symbol(char *ident, int length) bp = hash_tbl[h % PRIME]; } - /* recherche le mot cle */ + /* recherche le mot cle */ for (; bp != NULL; NEXT(bp)) { if (length == bp->length) { @@ -229,7 +229,7 @@ Index get_symbol(char *ident, int length) } } } - return (0); /* identificateur */ + return (0); /* identificateur */ } #endif diff --git a/modules/robot/src/wireframe-simulator/vpLex.cpp b/modules/robot/src/wireframe-simulator/vpLex.cpp index 7431716a6c..615a8d7db1 100644 --- a/modules/robot/src/wireframe-simulator/vpLex.cpp +++ b/modules/robot/src/wireframe-simulator/vpLex.cpp @@ -60,48 +60,48 @@ static void next_source(void); void lexerr(const char *path, ...); -/* Codes des symboles terminaux */ - -#define NULT 0 /* caractere non valide */ -#define EOBT 1 /* fin de buffer */ -#define EOFT 2 /* fin de fichier */ -#define EOLT 3 /* fin de ligne */ -#define CMTT 4 /* commentaire */ -#define IDNT 5 /* identificateur */ -#define INTT 6 /* nombre entier */ -#define FPTT 7 /* nombre flottant */ -#define SGNT 8 /* signe +/- */ -#define SPCT 9 /* caractere blanc */ -#define STGT 10 /* caractere de chaine */ -#define NBRT 11 /* nombre de codes */ - -/* Drapeaux des caracteres */ - -#define _NULT 0x00 /* caractere non valide */ -#define _CMTT 0x01 /* commentaire */ -#define _FPTT 0x02 /* nombre flottant */ -#define _IDNT 0x04 /* identificateur */ -#define _INTT 0x08 /* nombre entier */ -#define _SGNT 0x10 /* signe +/- */ -#define _STGT 0x20 /* caractere de chaine */ - -/* Caracteres sentinelles */ +/* Codes des symboles terminaux */ + +#define NULT 0 /* caractere non valide */ +#define EOBT 1 /* fin de buffer */ +#define EOFT 2 /* fin de fichier */ +#define EOLT 3 /* fin de ligne */ +#define CMTT 4 /* commentaire */ +#define IDNT 5 /* identificateur */ +#define INTT 6 /* nombre entier */ +#define FPTT 7 /* nombre flottant */ +#define SGNT 8 /* signe +/- */ +#define SPCT 9 /* caractere blanc */ +#define STGT 10 /* caractere de chaine */ +#define NBRT 11 /* nombre de codes */ + +/* Drapeaux des caracteres */ + +#define _NULT 0x00 /* caractere non valide */ +#define _CMTT 0x01 /* commentaire */ +#define _FPTT 0x02 /* nombre flottant */ +#define _IDNT 0x04 /* identificateur */ +#define _INTT 0x08 /* nombre entier */ +#define _SGNT 0x10 /* signe +/- */ +#define _STGT 0x20 /* caractere de chaine */ + +/* Caracteres sentinelles */ #define ASCII_NBR 128 /* nombre de codes ASCII*/ #ifndef EOB -#define EOB (-2) /* fin de buffer */ +#define EOB (-2) /* fin de buffer */ #endif #ifndef EOF -#define EOF (-1) /* fin de fichier */ +#define EOF (-1) /* fin de fichier */ #endif #ifndef EOL -#define EOL 10 /* fin de ligne */ +#define EOL 10 /* fin de ligne */ #endif -#define CHAR_NBR 130 /* nombre de caracteres */ +#define CHAR_NBR 130 /* nombre de caracteres */ -/* Tests des drapeaux */ +/* Tests des drapeaux */ #define isnult(c) (scantbl[c] == _NULT) #define iscmtt(c) (scantbl[c] & _CMTT) @@ -125,7 +125,7 @@ void lexerr(const char *path, ...); #define E_STRING 8 #define E_9 9 -const char *lex_errtbl[] = {/* table des messages d'erreur */ +const char *lex_errtbl[] = {/* table des messages d'erreur */ "error unknown", "symbol undefined", "unexpected EOF in comment", @@ -144,12 +144,12 @@ unsigned int mycolumno = 0; float myfloat = 0.0; int myint = 0; -static char *mysptr; /* tete de lecture de la ligne courante */ -static char *myline; /* debut de la ligne courante */ -static char *lastline; /* derniere ligne du buffer d'entree */ +static char *mysptr; /* tete de lecture de la ligne courante */ +static char *myline; /* debut de la ligne courante */ +static char *lastline; /* derniere ligne du buffer d'entree */ -static Byte *chtbl; /* premiers caracteres des terminaux */ -static Byte *scantbl; /* caracteres suivants des terminaux */ +static Byte *chtbl; /* premiers caracteres des terminaux */ +static Byte *scantbl; /* caracteres suivants des terminaux */ /* * La procedure "open_lex" alloue et initialise les variables utilisees @@ -163,10 +163,10 @@ void open_lex(void) perror(proc_name); throw vpException(vpException::fatalError, "Error in open_lex"); } - chtbl += 2; /* 2 sentinelles non affichables */ + chtbl += 2; /* 2 sentinelles non affichables */ scantbl += 2; - /* initialise les premiers caracteres des symboles terminaux */ + /* initialise les premiers caracteres des symboles terminaux */ for (int i = 0; i < ASCII_NBR; i++) { if (isalpha(i)) @@ -199,13 +199,13 @@ void open_lex(void) } } - /* Initialise les sentinelles comme des terminaux. */ + /* Initialise les sentinelles comme des terminaux. */ chtbl[EOB] = EOBT; chtbl[EOF] = EOFT; chtbl[EOL] = EOLT; - /* Initialise les caracteres suivants des symboles terminaux. */ + /* Initialise les caracteres suivants des symboles terminaux. */ for (int i = 0; i < ASCII_NBR; i++) { if (isalpha(i)) @@ -236,7 +236,7 @@ void open_lex(void) } } - /* Initialise les sentinelles comme des terminaux. */ + /* Initialise les sentinelles comme des terminaux. */ scantbl[EOB] = _NULT; scantbl[EOF] = _NULT; @@ -249,14 +249,14 @@ void open_lex(void) */ void close_lex(void) { - free((char *)(chtbl - 2)); /* voir "open_lex" pour "- 2" */ + free((char *)(chtbl - 2)); /* voir "open_lex" pour "- 2" */ free((char *)(scantbl - 2)); } #define ECHO printf("%c", *(mysptr)) -#define CURC (*((signed char *)mysptr)) /* caractere courant */ -#define NEXTC (*((signed char *)mysptr + 1)) /* caractere suivant */ -#define PREVC (*((signed char *)mysptr - 1)) /* caractere precedent */ +#define CURC (*((signed char *)mysptr)) /* caractere courant */ +#define NEXTC (*((signed char *)mysptr + 1)) /* caractere suivant */ +#define PREVC (*((signed char *)mysptr - 1)) /* caractere precedent */ /* * La procedure "lex" contient l'analyseur lexical. @@ -264,19 +264,19 @@ void close_lex(void) * La tete de lecture (mysptr) n'est pas systematiquement avancee apres *lecture. Le caractere courant est celui sous la tete de lecture. Ainsi on *accede de maniere symetrique aux caracteres precedent et suivant. Sortie : - * Code du symbole terminale analyse. + * Code du symbole terminale analyse. */ int lex(void) { lex_loop: for (; chtbl[(int)CURC] == SPCT; mysptr++) { - }; /* saute les espaces */ + }; /* saute les espaces */ switch (chtbl[(int)CURC]) { case NULT: - mytext = mysptr; /* sauvegarde le jeton */ + mytext = mysptr; /* sauvegarde le jeton */ mysptr++; return (*mytext); break; @@ -285,7 +285,7 @@ int lex(void) goto lex_loop; break; case EOFT: - mytext = mysptr; /* sauvegarde le jeton */ + mytext = mysptr; /* sauvegarde le jeton */ return (T_EOF); break; case EOLT: @@ -298,7 +298,7 @@ int lex(void) goto lex_loop; break; case CMTT: - mytext = mysptr; /* sauvegarde le jeton */ + mytext = mysptr; /* sauvegarde le jeton */ mysptr++; if (CURC != '*') return (*mytext); @@ -325,17 +325,17 @@ int lex(void) goto comment; break; case CMTT: - if (PREVC == '*') { /* veritable fin */ + if (PREVC == '*') { /* veritable fin */ mysptr++; goto lex_loop; } - mysptr++; /* pseudo fin */ + mysptr++; /* pseudo fin */ goto comment; break; } break; case IDNT: - mytext = mysptr; /* sauvegarde le jeton */ + mytext = mysptr; /* sauvegarde le jeton */ mysptr++; for (; isidnt((int)CURC); mysptr++) { }; @@ -343,14 +343,14 @@ int lex(void) return (get_symbol(mytext, mylength)); break; case INTT: - mytext = mysptr; /* sauvegarde le jeton */ + mytext = mysptr; /* sauvegarde le jeton */ int_part: myint = (int)(CURC - '0'); mysptr++; for (; isintt((int)CURC); mysptr++) myint = myint * 10 + (int)(CURC - '0'); switch (CURC) { - case '.': /* lecture fraction */ + case '.': /* lecture fraction */ float_part: mysptr++; for (; isintt((int)CURC); mysptr++) { @@ -363,7 +363,7 @@ int lex(void) return (T_FLOAT); } break; - case 'E': /* lecture exposant */ + case 'E': /* lecture exposant */ case 'e': mysptr++; if (isintt((int)CURC)) @@ -388,14 +388,14 @@ int lex(void) } break; case FPTT: - mytext = mysptr; /* sauvegarde le jeton */ + mytext = mysptr; /* sauvegarde le jeton */ mysptr++; - if (!isintt((int)CURC)) /* pas de fraction */ + if (!isintt((int)CURC)) /* pas de fraction */ return (*mytext); goto float_part; break; case SGNT: - mytext = mysptr; /* sauvegarde le jeton */ + mytext = mysptr; /* sauvegarde le jeton */ mysptr++; if (isintt((int)CURC)) goto int_part; @@ -404,7 +404,7 @@ int lex(void) return (*mytext); break; case STGT: - mytext = mysptr; /* sauvegarde le jeton */ + mytext = mysptr; /* sauvegarde le jeton */ mysptr++; string: for (; isstgt((int)CURC); mysptr++) { @@ -423,13 +423,13 @@ int lex(void) return ('\n'); break; case STGT: - if (PREVC != '\\') { /* veritable fin */ + if (PREVC != '\\') { /* veritable fin */ mytext++; mylength = (int)(mysptr - mytext); mysptr++; return (T_STRING); } - mysptr++; /* pseudo fin */ + mysptr++; /* pseudo fin */ goto string; break; } @@ -452,21 +452,21 @@ int lex(void) * La tete de lecture (mysptr) n'est pas systematiquement avancee apres *lecture. Le caractere courant est celui sous la tete de lecture. Ainsi on *accede de maniere symetrique aux caracteres precedent et suivant. Entree : - * f Fichier en sortie. - * token Jeton de fin de rechercher. + * f Fichier en sortie. + * token Jeton de fin de rechercher. * Sortie : - * Code du symbole terminale analyse. + * Code du symbole terminale analyse. */ int lexecho(FILE *f, int token) { lex_loop: - for (; chtbl[(int)CURC] == SPCT; mysptr++) /* saute les espaces */ + for (; chtbl[(int)CURC] == SPCT; mysptr++) /* saute les espaces */ fwrite(mysptr, 1, 1, f); switch (chtbl[(int)CURC]) { case NULT: - mytext = mysptr; /* sauvegarde le jeton */ + mytext = mysptr; /* sauvegarde le jeton */ mysptr++; if (token != *mytext) fwrite(mytext, 1, 1, f); @@ -477,7 +477,7 @@ int lexecho(FILE *f, int token) goto lex_loop; break; case EOFT: - mytext = mysptr; /* sauvegarde le jeton */ + mytext = mysptr; /* sauvegarde le jeton */ return (T_EOF); break; case EOLT: @@ -492,7 +492,7 @@ int lexecho(FILE *f, int token) break; case CMTT: fwrite(mysptr, 1, 1, f); - mytext = mysptr; /* sauvegarde le jeton */ + mytext = mysptr; /* sauvegarde le jeton */ mysptr++; if (CURC != '*') return (*mytext); @@ -522,17 +522,17 @@ int lexecho(FILE *f, int token) break; case CMTT: fwrite(mysptr, 1, 1, f); - if (PREVC == '*') { /* veritable fin */ + if (PREVC == '*') { /* veritable fin */ mysptr++; goto lex_loop; } - mysptr++; /* pseudo fin */ + mysptr++; /* pseudo fin */ goto comment; break; } break; case IDNT: - mytext = mysptr; /* sauvegarde le jeton */ + mytext = mysptr; /* sauvegarde le jeton */ mysptr++; for (; isidnt((int)CURC); mysptr++) { }; @@ -542,13 +542,13 @@ int lexecho(FILE *f, int token) return (get_symbol(mytext, mylength)); break; case INTT: - mytext = mysptr; /* sauvegarde le jeton */ + mytext = mysptr; /* sauvegarde le jeton */ int_part: mysptr++; for (; isintt((int)CURC); mysptr++) { }; switch (CURC) { - case '.': /* lecture fraction */ + case '.': /* lecture fraction */ float_part: mysptr++; for (; isintt((int)CURC); mysptr++) { @@ -559,7 +559,7 @@ int lexecho(FILE *f, int token) return (T_FLOAT); } break; - case 'E': /* lecture exposant */ + case 'E': /* lecture exposant */ case 'e': mysptr++; if (isintt((int)CURC)) @@ -586,9 +586,9 @@ int lexecho(FILE *f, int token) } break; case FPTT: - mytext = mysptr; /* sauvegarde le jeton */ + mytext = mysptr; /* sauvegarde le jeton */ mysptr++; - if (!isintt((int)CURC)) { /* pas de fraction */ + if (!isintt((int)CURC)) { /* pas de fraction */ if (token != *mytext) fwrite(mytext, 1, 1, f); return (*mytext); @@ -596,7 +596,7 @@ int lexecho(FILE *f, int token) goto float_part; break; case SGNT: - mytext = mysptr; /* sauvegarde le jeton */ + mytext = mysptr; /* sauvegarde le jeton */ mysptr++; if (isintt((int)CURC)) goto int_part; @@ -608,7 +608,7 @@ int lexecho(FILE *f, int token) break; case STGT: fwrite(mysptr, 1, 1, f); - mytext = mysptr; /* sauvegarde le jeton */ + mytext = mysptr; /* sauvegarde le jeton */ mysptr++; string: for (; isstgt((int)CURC); mysptr++) @@ -628,13 +628,13 @@ int lexecho(FILE *f, int token) break; case STGT: fwrite(mysptr, 1, 1, f); - if (PREVC != '\\') { /* veritable fin */ + if (PREVC != '\\') { /* veritable fin */ mytext++; mylength = (int)(mysptr - mytext); mysptr++; return (T_STRING); } - mysptr++; /* pseudo fin */ + mysptr++; /* pseudo fin */ goto string; break; } @@ -656,11 +656,11 @@ int lexecho(FILE *f, int token) #define LINESIZE (BUFSIZ - 1) #define TEXTSIZE (1 + LINESIZE + BUFSIZE + 1) -static FILE *fds; /* descripteur du fichier source */ -static char *source; /* nom du fichier du programme source */ -static char *botbuf; /* fond du buffer d'entree du fichier */ -static char *buf; /* base du buffer d'entree du fichier */ -static char *topbuf; /* sommet du buffer d'entree du fichier */ +static FILE *fds; /* descripteur du fichier source */ +static char *source; /* nom du fichier du programme source */ +static char *botbuf; /* fond du buffer d'entree du fichier */ +static char *buf; /* base du buffer d'entree du fichier */ +static char *topbuf; /* sommet du buffer d'entree du fichier */ /* * La procedure "unlex" recule la tete de lecture devant le dernier jeton. @@ -671,8 +671,8 @@ void unlex(void) { mysptr = mytext; } * La procedure "open_source" alloue et initialise les variables utilisees * pour la gestion des entrees du programme source. * Entree : - * fd Fichier du programme source. - * sptr Nom du fichier du programme source. + * fd Fichier du programme source. + * sptr Nom du fichier du programme source. */ void open_source(FILE *fd, const char *str) { @@ -713,9 +713,9 @@ static void next_source(void) char *bot = buf; char *top = topbuf; - /* recopie la derniere ligne devant "buf" */ + /* recopie la derniere ligne devant "buf" */ - *bot = EOL; /* evite le debordement de "buf" */ + *bot = EOL; /* evite le debordement de "buf" */ while ((*--bot = *--top) != EOL) { }; myline = mysptr = bot + 1; @@ -724,13 +724,13 @@ static void next_source(void) if (size == 0) { topbuf = buf + 1; *buf = EOF; - *topbuf = EOB; /* sentinelle de fin de fichier */ + *topbuf = EOB; /* sentinelle de fin de fichier */ mysptr = buf; } else { topbuf = buf + size; - *topbuf = EOB; /* sentinelle de fin de buffer */ + *topbuf = EOB; /* sentinelle de fin de buffer */ - /* recherche de la derniere ligne */ + /* recherche de la derniere ligne */ top = topbuf; while (*--top != EOL) { }; @@ -739,7 +739,7 @@ static void next_source(void) } /* - * ERR_STACK : Pile des messages d'erreur. + * ERR_STACK : Pile des messages d'erreur. * La pile est geree par les procedures "poperr", "popuperr" et "pusherr". * Les messages sont affiches par les procedures "count" et "lexerr". */ @@ -769,7 +769,7 @@ static void count(void) * 3 elle affiche les messages d'erreur contenus dans la pile. * 4 elle affiche les messages d'erreur en parametre. * Entree : - * va_list Liste de messages d'erreur terminee par NULL. + * va_list Liste de messages d'erreur terminee par NULL. */ // lexerr (va_alist) @@ -781,18 +781,18 @@ void lexerr(const char *path, ...) char *cp; int i; - /* Pointe sur le caractere fautif. */ + /* Pointe sur le caractere fautif. */ count(); // write (STDERR, myline, mysptr - myline); fprintf(stderr, "\n%*c\n\"%s\", line %d:\n", mycolumno, '^', source, mylineno); - /* Affiche les messages d'erreur de la pile. */ + /* Affiche les messages d'erreur de la pile. */ for (i = 0; i < size_stack; i++) fprintf(stderr, "%s", err_stack[i]); - /* Affiche les messages d'erreur en parametres. */ + /* Affiche les messages d'erreur en parametres. */ va_start(ap, path); while ((cp = (char *)va_arg(ap, char *)) != NULL) diff --git a/modules/robot/src/wireframe-simulator/vpMy.h b/modules/robot/src/wireframe-simulator/vpMy.h index ae285c8bf6..ec3cb0f6f6 100644 --- a/modules/robot/src/wireframe-simulator/vpMy.h +++ b/modules/robot/src/wireframe-simulator/vpMy.h @@ -69,12 +69,7 @@ #define M_EPSILON 1E-06 -//#define ABS(X) (((X) < 0) ? -(X) : (X)) #define FABS(X) (((X) < 0.0) ? -(X) : (X)) -//#define MAX(A,B) (((A) > (B)) ? (A) : (B)) -//#define MAX3(A,B,C) (MAX(MAX(A,B),C)) -//#define MIN(A,B) (((A) < (B)) ? (A) : (B)) -//#define MIN3(A,B,C) (MIN(MIN(A,B),C)) #define MIN_MAX(M, MIN, MAX) \ if ((M) < (MIN)) \ diff --git a/modules/robot/src/wireframe-simulator/vpMyio.cpp b/modules/robot/src/wireframe-simulator/vpMyio.cpp index e63044e386..0455bffe9c 100644 --- a/modules/robot/src/wireframe-simulator/vpMyio.cpp +++ b/modules/robot/src/wireframe-simulator/vpMyio.cpp @@ -50,12 +50,12 @@ #ifndef DOXYGEN_SHOULD_SKIP_THIS -extern char *mytext; /* chaine du symbole courant */ +extern char *mytext; /* chaine du symbole courant */ /* * La procedure "fscanf_float" lit en ascii un nombre flottant. * Entree : - * fp Nombre flottant a lire. + * fp Nombre flottant a lire. */ void fscanf_float(float *fp) { @@ -69,7 +69,7 @@ void fscanf_float(float *fp) /* * La procedure "fscanf_Index" lit en ascii un indice. * Entree : - * ip Indice a lire. + * ip Indice a lire. */ void fscanf_Index(Index *ip) { @@ -81,7 +81,7 @@ void fscanf_Index(Index *ip) /* * La procedure "fscanf_int" lit en ascii un nombre entier. * Entree : - * ip Nombre entier a lire. + * ip Nombre entier a lire. */ void fscanf_int(int *ip) { @@ -93,7 +93,7 @@ void fscanf_int(int *ip) /* * La procedure "fscanf_string" lit en ascii une chaine de caracteres. * Entree : - * str Chaine a lire. + * str Chaine a lire. */ void fscanf_string(char **str) { @@ -115,7 +115,7 @@ void fscanf_string(char **str) /* * La procedure "fscanf_Type" lit en ascii un octet. * Entree : - * ip Type a lire. + * ip Type a lire. */ void fscanf_Type(Type *ip) { diff --git a/modules/robot/src/wireframe-simulator/vpParser.cpp b/modules/robot/src/wireframe-simulator/vpParser.cpp index 1153256e96..4bbd3b4cd6 100644 --- a/modules/robot/src/wireframe-simulator/vpParser.cpp +++ b/modules/robot/src/wireframe-simulator/vpParser.cpp @@ -50,7 +50,7 @@ /* * La procedure "parser" fait l'analyse syntaxique du fichier source. * Entree/Sortie : - * bsp Scene surfacique polygonale a lire. + * bsp Scene surfacique polygonale a lire. */ void parser(Bound_scene *bsp) { @@ -60,7 +60,7 @@ void parser(Bound_scene *bsp) switch (token) { case '$': switch (lex()) { - case T_IDENT: /* saute la commande inconnue */ + case T_IDENT: /* saute la commande inconnue */ skip_cmd(/* stderr */); unlex(); break; @@ -82,7 +82,7 @@ void parser(Bound_scene *bsp) fscanf_View_parameters(get_view_parameters()); set_projection(void); break; -#endif /* used */ +#endif /* used */ default: lexerr("start", "keyword expected", NULL); break; diff --git a/modules/robot/src/wireframe-simulator/vpProjection.cpp b/modules/robot/src/wireframe-simulator/vpProjection.cpp index 97af381fd6..fd08273c39 100644 --- a/modules/robot/src/wireframe-simulator/vpProjection.cpp +++ b/modules/robot/src/wireframe-simulator/vpProjection.cpp @@ -48,8 +48,8 @@ * La procedure "View_to_Matrix" constuit la matrice homogene de projection * a partir des parametres de la prise de vue. * Entree : - * vp Parametres de la prise de vue. - * m Matrice homogene a construire. + * vp Parametres de la prise de vue. + * m Matrice homogene a construire. */ void View_to_Matrix(View_parameters *vp, Matrix m) { @@ -71,12 +71,12 @@ void View_to_Matrix(View_parameters *vp, Matrix m) /* * La procedure "set_zy" initialise la matrice par une composition : - * 1 - aligne le premier vecteur sur l'axe Z dans le sens negatif. - * 2 - aligne la projection du second vecteur sur l'axe Y. + * 1 - aligne le premier vecteur sur l'axe Z dans le sens negatif. + * 2 - aligne la projection du second vecteur sur l'axe Y. * Entree : - * m Matrice a initialiser. - * v0 Premier vecteur. - * v1 Second vecteur. + * m Matrice a initialiser. + * v0 Premier vecteur. + * v1 Second vecteur. */ static void set_zy(Matrix m, Vector *v0, Vector *v1) { @@ -86,7 +86,7 @@ static void set_zy(Matrix m, Vector *v0, Vector *v1) CROSS_PRODUCT(rx, *v0, *v1); norm_vector(&rx); norm_vector(&rz); - CROSS_PRODUCT(ry, rz, rx); /* ry est norme */ + CROSS_PRODUCT(ry, rz, rx); /* ry est norme */ m[0][0] = rx.x; m[0][1] = ry.x; @@ -110,11 +110,11 @@ static void set_zy(Matrix m, Vector *v0, Vector *v1) * La procedure "set_parallel" iniatilise la matrice de projection * parallel "wc" par les parametres de visualisation "vp". * Pour plus de renseignements : - * "Fundamentals of Interactive Computer Graphics" - * J.D. FOLEY, A. VAN DAM, Addison-Wesley. 1982, pp 285-290. + * "Fundamentals of Interactive Computer Graphics" + * J.D. FOLEY, A. VAN DAM, Addison-Wesley. 1982, pp 285-290. * Entree : - * vp Parametres de visualisation. - * wc Matrice a initialiser. + * vp Parametres de visualisation. + * wc Matrice a initialiser. */ void set_parallel(View_parameters *vp, Matrix wc) { @@ -157,9 +157,9 @@ void set_parallel(View_parameters *vp, Matrix wc) /* * 6 : Translation et Mise a l'echelle de la pyramide. * Remarque : contrairement a la reference qui donne - * 0 < x < 1, 0 < y < 1, 0 < z < 1 + * 0 < x < 1, 0 < y < 1, 0 < z < 1 * je prefere, afin de rester coherent avec la projection perspective, - * -1 < x < 1, -1 < y < 1, 0 < z < 1 (w = 1) + * -1 < x < 1, -1 < y < 1, 0 < z < 1 (w = 1) */ SET_COORD3(v, (float)(-(vp->vwd.umax + vp->vwd.umin) / 2.0), (float)(-(vp->vwd.vmax + vp->vwd.vmin) / 2.0), (float)(-vp->depth.front)); @@ -173,11 +173,11 @@ void set_parallel(View_parameters *vp, Matrix wc) * La procedure "set_perspective" iniatilise la matrice de projection * perspective "wc" par les parametres de visualisation "vp". * Pour plus de renseignements : - * "Fundamentals of Interactive Computer Graphics" - * J.D. FOLEY, A. VAN DAM, Addison-Wesley. 1982, pp 290-302. + * "Fundamentals of Interactive Computer Graphics" + * J.D. FOLEY, A. VAN DAM, Addison-Wesley. 1982, pp 290-302. * Entree : - * vp Parametres de visualisation. - * wc Matrice a initialiser. + * vp Parametres de visualisation. + * wc Matrice a initialiser. */ void set_perspective(View_parameters *vp, Matrix wc) { diff --git a/modules/robot/src/wireframe-simulator/vpRfstack.cpp b/modules/robot/src/wireframe-simulator/vpRfstack.cpp index 432e9184c8..dbac55bd7b 100644 --- a/modules/robot/src/wireframe-simulator/vpRfstack.cpp +++ b/modules/robot/src/wireframe-simulator/vpRfstack.cpp @@ -48,19 +48,19 @@ #include #define STACKSIZE 32 -static int stack[STACKSIZE] = {vpDEFAULT_REMOVE}; /* pile */ -static int *sp = stack; /* sommet */ +static int stack[STACKSIZE] = {vpDEFAULT_REMOVE}; /* pile */ +static int *sp = stack; /* sommet */ /* * La procedure "fprintf_rfstack" affiche le sommet * de la pile des drapeaux d'elimination de faces. * Entree : - * fp Fichier en sortie. + * fp Fichier en sortie. */ void fprintf_rfstack(FILE *fp) { int flg; - flg = 0; /* nul si element unique */ + flg = 0; /* nul si element unique */ if (*sp == IS_INSIDE) { fprintf(fp, "(null)\n"); @@ -108,7 +108,7 @@ void fprintf_rfstack(FILE *fp) * La procedure "get_rfstack" retourne les drapeaux au sommet * de la pile des drapeaux d'elimination de faces. * Sortie : - * Pointeur sur les drapeaux d'elimination du sommet de la pile. + * Pointeur sur les drapeaux d'elimination du sommet de la pile. */ int *get_rfstack(void) { return (sp); } @@ -116,7 +116,7 @@ int *get_rfstack(void) { return (sp); } * La procedure "load_rfstack" charge des drapeaux au sommet * de la pile des drapeaux d'elimination de faces. * Entree : - * i Niveau a charger. + * i Niveau a charger. */ void load_rfstack(int i) { *sp = i; } diff --git a/modules/robot/src/wireframe-simulator/vpSkipio.cpp b/modules/robot/src/wireframe-simulator/vpSkipio.cpp index c78965381f..a58d5ea2b1 100644 --- a/modules/robot/src/wireframe-simulator/vpSkipio.cpp +++ b/modules/robot/src/wireframe-simulator/vpSkipio.cpp @@ -51,7 +51,7 @@ * La procedure "skip_cmd" saute les structures d'une commande * jusqu'a reconnaitre le debut d'une nouvelle commande. * Entree : - * f Fichier en sortie. + * f Fichier en sortie. */ void skip_cmd(void) { @@ -68,19 +68,19 @@ void skip_cmd(void) * La procedure "skip_keyword" saute les structures des articles * jusqu'a reconnaitre le mot cle de jeton "token". * Entree : - * token Jeton du mot cle a reconnaitre. - * err Message d'erreur si le mot cle n'est pas reconnu. + * token Jeton du mot cle a reconnaitre. + * err Message d'erreur si le mot cle n'est pas reconnu. */ void skip_keyword(int token, const char *err) { int t; switch (t = lex()) { - case T_IDENT: /* saute le mot cle inconnu */ + case T_IDENT: /* saute le mot cle inconnu */ while ((t = lex()) != 0) { switch (t) { - case '$': /* nouvelle commande */ - case T_EOF: /* fin de fichier */ + case '$': /* nouvelle commande */ + case T_EOF: /* fin de fichier */ lexerr("start", err, NULL); break; default: diff --git a/modules/robot/src/wireframe-simulator/vpTmstack.cpp b/modules/robot/src/wireframe-simulator/vpTmstack.cpp index dd1e3307c9..f93f3beb8f 100644 --- a/modules/robot/src/wireframe-simulator/vpTmstack.cpp +++ b/modules/robot/src/wireframe-simulator/vpTmstack.cpp @@ -50,14 +50,14 @@ #define STACKSIZE 32 -static Matrix stack[STACKSIZE] /* = IDENTITY_MATRIX*/; /* pile */ -static Matrix *sp = stack; /* sommet */ +static Matrix stack[STACKSIZE] /* = IDENTITY_MATRIX*/; /* pile */ +static Matrix *sp = stack; /* sommet */ /* * La procedure "get_tmstack" retourne la matrice au sommet * de la pile des matrices de transformation. * Sortie : - * Pointeur de la matrice au sommet de la pile. + * Pointeur de la matrice au sommet de la pile. */ Matrix *get_tmstack(void) { return (sp); } @@ -65,7 +65,7 @@ Matrix *get_tmstack(void) { return (sp); } * La procedure "load_tmstack" charge une matrice au sommet * de la pile des matrices de transformation. * Entree : - * m Matrice a charger. + * m Matrice a charger. */ void load_tmstack(Matrix m) { @@ -112,9 +112,9 @@ void swap_tmstack(void) Matrix *mp, tmp; mp = (sp == stack) ? sp + 1 : sp - 1; - // bcopy ((char *) *sp, (char *) tmp, sizeof (Matrix)); - // bcopy ((char *) *mp, (char *) *sp, sizeof (Matrix)); - // bcopy ((char *) tmp, (char *) *mp, sizeof (Matrix)); + // bcopy ((char *) *sp, (char *) tmp, sizeof (Matrix)); + // bcopy ((char *) *mp, (char *) *sp, sizeof (Matrix)); + // bcopy ((char *) tmp, (char *) *mp, sizeof (Matrix)); memmove((char *)tmp, (char *)*sp, sizeof(Matrix)); memmove((char *)*sp, (char *)*mp, sizeof(Matrix)); memmove((char *)*mp, (char *)tmp, sizeof(Matrix)); @@ -124,7 +124,7 @@ void swap_tmstack(void) * La procedure "postmult_tmstack" postmultiplie la matrice au sommet * de la pile des matrices de transformation. * Entree : - * m Matrice multiplicative. + * m Matrice multiplicative. */ void postmult_tmstack(Matrix m) { postmult_matrix(*sp, m); } @@ -132,7 +132,7 @@ void postmult_tmstack(Matrix m) { postmult_matrix(*sp, m); } * La procedure "postrotate_tmstack" postmultiplie la matrice au sommet * de la pile des matrices de transformation par une rotation. * Entree : - * vp Vecteur de rotation. + * vp Vecteur de rotation. */ void postrotate_tmstack(Vector *vp) { @@ -146,7 +146,7 @@ void postrotate_tmstack(Vector *vp) * La procedure "postscale_tmstack" postmultiplie la matrice au sommet * de la pile des matrices de transformation par une homothetie. * Entree : - * vp Vecteur d'homothetie. + * vp Vecteur d'homothetie. */ void postscale_tmstack(Vector *vp) { postscale_matrix(*sp, vp); } @@ -154,7 +154,7 @@ void postscale_tmstack(Vector *vp) { postscale_matrix(*sp, vp); } * La procedure "posttranslate_tmstack" postmultiplie la matrice au sommet * de la pile des matrices de transformation par une translation. * Entree : - * vp Vecteur de translation. + * vp Vecteur de translation. */ void posttranslate_tmstack(Vector *vp) { posttrans_matrix(*sp, vp); } @@ -162,7 +162,7 @@ void posttranslate_tmstack(Vector *vp) { posttrans_matrix(*sp, vp); } * La procedure "premult_tmstack" premultiplie la matrice au sommet * de la pile des matrices de transformation. * Entree : - * m Matrice multiplicative. + * m Matrice multiplicative. */ void premult_tmstack(Matrix m) { premult_matrix(*sp, m); } @@ -170,7 +170,7 @@ void premult_tmstack(Matrix m) { premult_matrix(*sp, m); } * La procedure "prerotate_tmstack" premultiplie la matrice au sommet * de la pile des matrices de transformation par une rotation. * Entree : - * vp Vecteur de rotation. + * vp Vecteur de rotation. */ void prerotate_tmstack(Vector *vp) { @@ -184,7 +184,7 @@ void prerotate_tmstack(Vector *vp) * La procedure "prescale_tmstack" premultiplie la matrice au sommet * de la pile des matrices de transformation par une homothetie. * Entree : - * vp Vecteur d'homothetie. + * vp Vecteur d'homothetie. */ void prescale_tmstack(Vector *vp) { prescale_matrix(*sp, vp); } @@ -192,7 +192,7 @@ void prescale_tmstack(Vector *vp) { prescale_matrix(*sp, vp); } * La procedure "pretranslate_tmstack" premultiplie la matrice au sommet * de la pile des matrices de transformation par une translation. * Entree : - * vp Vecteur de translation. + * vp Vecteur de translation. */ void pretranslate_tmstack(Vector *vp) { pretrans_matrix(*sp, vp); } diff --git a/modules/robot/src/wireframe-simulator/vpToken.h b/modules/robot/src/wireframe-simulator/vpToken.h index 05305c204f..dd6a8cab8f 100644 --- a/modules/robot/src/wireframe-simulator/vpToken.h +++ b/modules/robot/src/wireframe-simulator/vpToken.h @@ -46,8 +46,8 @@ #include typedef struct { - const char *ident; /* identifateur */ - Index token; /* code du jeton */ + const char *ident; /* identifateur */ + Index token; /* code du jeton */ } Keyword; #define T_EOF 256 diff --git a/modules/robot/src/wireframe-simulator/vpView.h b/modules/robot/src/wireframe-simulator/vpView.h index 91a2ff2c2d..9ce5c8b600 100644 --- a/modules/robot/src/wireframe-simulator/vpView.h +++ b/modules/robot/src/wireframe-simulator/vpView.h @@ -52,12 +52,12 @@ * - Les 6 plans de clipping definissent le volume canonique * de la pyramide de vision dans lequel la scene est visible. * - les 6 plans ont pour equations : - * Plan dessus : W = Y - * Plan dessous : -W = Y - * Plan droit : W = X - * Plan gauche : -W = X - * Plan arriere : W = Z - * Plan avant : W = 0 + * Plan dessus : W = Y + * Plan dessous : -W = Y + * Plan droit : W = X + * Plan gauche : -W = X + * Plan arriere : W = Z + * Plan avant : W = 0 */ #define PLANE_ABOVE 0 #define PLANE_BELOW 1 @@ -140,44 +140,44 @@ } /* - * CAMERA PARAMETERS - * _________________ + * CAMERA PARAMETERS + * _________________ * * La structure "Camera_parameters" definit les parametres de la camera. - * eye Position de l'oeil ou de la camera. - * target Position de la cible ou du point visee dans la scene. - * focal Distance eye-target - * angle Angle d'ouverture en degres. - * twist Angle de rotation sur l'axe de visee (eye,target) en degres. - * speed Vitesse sur l'axe de visee (eye,target). + * eye Position de l'oeil ou de la camera. + * target Position de la cible ou du point visee dans la scene. + * focal Distance eye-target + * angle Angle d'ouverture en degres. + * twist Angle de rotation sur l'axe de visee (eye,target) en degres. + * speed Vitesse sur l'axe de visee (eye,target). */ typedef struct { - Point3f eye; /* position de l'observateur */ - Point3f target; /* point vise */ - float focal; /* focale de la camera */ - float angle; /* angle d'ouverture */ - float twist; /* rotation sur l'axe de visee */ - float speed; /* vitesse sur l'axe de visee */ + Point3f eye; /* position de l'observateur */ + Point3f target; /* point vise */ + float focal; /* focale de la camera */ + float angle; /* angle d'ouverture */ + float twist; /* rotation sur l'axe de visee */ + float speed; /* vitesse sur l'axe de visee */ } Camera_parameters; typedef struct { - float umin, umax; /* bords gauche et droit */ + float umin, umax; /* bords gauche et droit */ float vmin, vmax; /* bords inferieur et superieur */ } View_window; typedef struct { - float front; /* plan avant ("hither") */ - float back; /* plan arriere ("yon") */ + float front; /* plan avant ("hither") */ + float back; /* plan arriere ("yon") */ } View_depth; typedef struct { - Type type; /* type de la projection */ - Point3f cop; /* centre de projection */ - Point3f vrp; /* point de reference de visee */ - Vector vpn; /* vecteur nomal au plan */ - Vector vup; /* vecteur indiquant le "haut" */ - View_window vwd; /* fenetre de projection */ - View_depth depth; /* profondeurs de decoupages */ + Type type; /* type de la projection */ + Point3f cop; /* centre de projection */ + Point3f vrp; /* point de reference de visee */ + Vector vpn; /* vecteur nomal au plan */ + Vector vup; /* vecteur indiquant le "haut" */ + View_window vwd; /* fenetre de projection */ + View_depth depth; /* profondeurs de decoupages */ } View_parameters; #endif diff --git a/modules/robot/src/wireframe-simulator/vpViewio.cpp b/modules/robot/src/wireframe-simulator/vpViewio.cpp index c2cd51d2b5..29bd02a610 100644 --- a/modules/robot/src/wireframe-simulator/vpViewio.cpp +++ b/modules/robot/src/wireframe-simulator/vpViewio.cpp @@ -57,7 +57,7 @@ * La procedure "fscanf_Remove" lit en ascii les parametres d'elimination * des faces. * Entree : - * bp Parametres a lire. + * bp Parametres a lire. */ void fscanf_Remove(Byte *bp) { @@ -94,11 +94,11 @@ void fscanf_Remove(Byte *bp) * La procedure "fscanf_View_parameters" lit en ascii les parametres * de visualisation. * Entree : - * vp Parametres de visualisation a lire. + * vp Parametres de visualisation a lire. */ void fscanf_View_parameters(View_parameters *vp) { - /* Lecture du type de projection lors de la prise de vue. */ + /* Lecture du type de projection lors de la prise de vue. */ skip_keyword(T_TYPE, "view: keyword \"type\" expected"); switch (lex()) { @@ -113,35 +113,35 @@ void fscanf_View_parameters(View_parameters *vp) break; } - /* Lecture du centre de projection (oeil) de la prise de vue. */ + /* Lecture du centre de projection (oeil) de la prise de vue. */ skip_keyword(T_COP, "view: keyword \"cop\" expected"); pusherr("view_cop: "); fscanf_Point3f(&vp->cop); poperr(); - /* Lecture du point de reference (cible) a la prise de vue. */ + /* Lecture du point de reference (cible) a la prise de vue. */ skip_keyword(T_VRP, "view: keyword \"vrp\" expected"); pusherr("view_vrp: "); fscanf_Point3f(&vp->vrp); poperr(); - /* Lecture de la direction normale au plan de projection. */ + /* Lecture de la direction normale au plan de projection. */ skip_keyword(T_VPN, "view: keyword \"vpn\" expected"); pusherr("view_vpn: "); fscanf_Vector(&vp->vpn); poperr(); - /* Lecture de la direction indiquant le haut de la projection. */ + /* Lecture de la direction indiquant le haut de la projection. */ skip_keyword(T_VUP, "view: keyword \"vup\" expected"); pusherr("view_vup: "); fscanf_Vector(&vp->vup); poperr(); - /* Lecture de la fenetre de projection de la prise de vue. */ + /* Lecture de la fenetre de projection de la prise de vue. */ skip_keyword(T_WINDOW, "view: keyword \"window\" expected"); pusherr("view_window_umin: "); @@ -154,7 +154,7 @@ void fscanf_View_parameters(View_parameters *vp) fscanf_float(&vp->vwd.vmax); poperr(); - /* Lecture des profondeurs de decoupage avant et arriere. */ + /* Lecture des profondeurs de decoupage avant et arriere. */ skip_keyword(T_DEPTH, "view: keyword \"depth\" expected"); pusherr("view_depth_front: "); diff --git a/modules/robot/src/wireframe-simulator/vpVwstack.cpp b/modules/robot/src/wireframe-simulator/vpVwstack.cpp index e8e139cbe5..2bf5ec895c 100644 --- a/modules/robot/src/wireframe-simulator/vpVwstack.cpp +++ b/modules/robot/src/wireframe-simulator/vpVwstack.cpp @@ -61,9 +61,9 @@ static View_parameters *sp = stack; * La procedure "fprintf_vwstack" affiche un parametre du sommet * de la pile des prises de vue. * Entree : - * fp Fichier de sortie. - * argv Argument a afficher. - * Si argv est nul, tous les parametres sont affiches. + * fp Fichier de sortie. + * argv Argument a afficher. + * Si argv est nul, tous les parametres sont affiches. */ void fprintf_vwstack(FILE *fp, char *argv) { @@ -125,7 +125,7 @@ void fprintf_vwstack(FILE *fp, char *argv) * La procedure "get_vwstack" retourne le point de vue au sommet * de la pile des points de vue. * Sortie : - * Pointeur sur le point de vue du sommet de la pile. + * Pointeur sur le point de vue du sommet de la pile. */ View_parameters *get_vwstack(void) { return (sp); } @@ -133,7 +133,7 @@ View_parameters *get_vwstack(void) { return (sp); } * La procedure "load_vwstack" charge un point de vue au sommet * de la pile des points de vue. * Entree : - * vp Point de vue a charger. + * vp Point de vue a charger. */ void load_vwstack(View_parameters *vp) { *sp = *vp; } @@ -182,7 +182,7 @@ void swap_vwstack(void) * La procedure "add_vwstack" modifie un agrument du point de vue au sommet * de la pile des points de vue. * Entree : - * va_alist Nom de l'argument a modifier suivi de ses parametres. + * va_alist Nom de l'argument a modifier suivi de ses parametres. */ void add_vwstack(const char *path, ...) @@ -195,17 +195,17 @@ void add_vwstack(const char *path, ...) va_start(ap, path); argv = va_arg(ap, char *); if (strcmp(argv, "cop") == 0) { - /* initialise le centre de projection */ + /* initialise le centre de projection */ SET_COORD3(sp->cop, (float)va_arg(ap, double), (float)va_arg(ap, double), (float)va_arg(ap, double)); } else if (strcmp(argv, "depth") == 0) { - /* initialise les distances des plans de decoupage */ + /* initialise les distances des plans de decoupage */ sp->depth.front = (float)va_arg(ap, double); sp->depth.back = (float)va_arg(ap, double); } else if (strcmp(argv, "type") == 0) { - /* initialise le type de projection */ + /* initialise le type de projection */ sp->type = (Type)va_arg(ap, int); } else if (strcmp(argv, "vpn") == 0) { - /* initialise le vecteur normal au plan */ + /* initialise le vecteur normal au plan */ float x = (float)va_arg(ap, double); float y = (float)va_arg(ap, double); float z = (float)va_arg(ap, double); @@ -220,10 +220,10 @@ void add_vwstack(const char *path, ...) SET_COORD3(sp->vpn, x, y, z); } } else if (strcmp(argv, "vrp") == 0) { - /* initialise le vecteur de reference */ + /* initialise le vecteur de reference */ SET_COORD3(sp->vrp, (float)va_arg(ap, double), (float)va_arg(ap, double), (float)va_arg(ap, double)); } else if (strcmp(argv, "vup") == 0) { - /* initialise le vecteur haut du plan */ + /* initialise le vecteur haut du plan */ float x = (float)va_arg(ap, double); float y = (float)va_arg(ap, double); float z = (float)va_arg(ap, double); @@ -238,7 +238,7 @@ void add_vwstack(const char *path, ...) SET_COORD3(sp->vup, x, y, z); } } else if (strcmp(argv, "window") == 0) { - /* initialise la fenetre de projection */ + /* initialise la fenetre de projection */ sp->vwd.umin = (float)va_arg(ap, double); sp->vwd.umax = (float)va_arg(ap, double); sp->vwd.vmin = (float)va_arg(ap, double); diff --git a/modules/sensor/include/visp3/sensor/vpScanPoint.h b/modules/sensor/include/visp3/sensor/vpScanPoint.h index 95fceee0ab..9222320201 100644 --- a/modules/sensor/include/visp3/sensor/vpScanPoint.h +++ b/modules/sensor/include/visp3/sensor/vpScanPoint.h @@ -156,9 +156,6 @@ class /* VISP_EXPORT */ vpScanPoint // Note that here VISP_EXPORT should not */ friend inline bool operator==(const vpScanPoint &sp1, const vpScanPoint &sp2) { - // return ( ( sp1.getRadialDist() == sp2.getRadialDist() ) - // && ( sp1.getHAngle() == sp2.getHAngle() ) - // && ( sp1.getVAngle() == sp2.getVAngle() ) ); double rd1 = sp1.getRadialDist(); double ha1 = sp1.getHAngle(); double va1 = sp1.getVAngle(); diff --git a/modules/sensor/src/force-torque/vpForceTorqueIitSensor.cpp b/modules/sensor/src/force-torque/vpForceTorqueIitSensor.cpp index 1011f1abdd..58b4084eb2 100644 --- a/modules/sensor/src/force-torque/vpForceTorqueIitSensor.cpp +++ b/modules/sensor/src/force-torque/vpForceTorqueIitSensor.cpp @@ -186,7 +186,7 @@ bool vpForceTorqueIitSensor::connected(int timeout_ms) const \param[in] filtered : When true return filtered force-torque measurements, when false return raw data. If no filter is configured while getting filtered measurements, the SDK will - retun the raw data. + return the raw data. To configure the filter, you must access the sensor through the web interface. The default ip address is `192.168.1.1` if in default mode. Once in the web interface select NETWORK SETTINGS and you can configure the diff --git a/modules/sensor/src/framegrabber/1394/vp1394TwoGrabber.cpp b/modules/sensor/src/framegrabber/1394/vp1394TwoGrabber.cpp index d7940ac9ef..512e4fa2ab 100644 --- a/modules/sensor/src/framegrabber/1394/vp1394TwoGrabber.cpp +++ b/modules/sensor/src/framegrabber/1394/vp1394TwoGrabber.cpp @@ -2006,45 +2006,37 @@ void vp1394TwoGrabber::setIsoTransmissionSpeed(vp1394TwoIsoSpeedType isospeed) // Check the speed to configure in B-mode or A-mode if (isospeed >= vpISO_SPEED_800) { if (camera->bmode_capable != DC1394_TRUE) { - // vpERROR_TRACE("Camera is not 1394B mode capable. \n" - // "Set the iso speed lower or equal to 400Mbps"); close(); throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Camera is not 1394B mode capable")); } if (dc1394_video_set_operation_mode(camera, DC1394_OPERATION_MODE_1394B) != DC1394_SUCCESS) { - // vpERROR_TRACE("Cannot set camera to 1394B mode. \n"); close(); throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Cannot set camera to 1394B mode")); } if (dc1394_video_get_operation_mode(camera, &op_mode) != DC1394_SUCCESS) { - // vpERROR_TRACE("Failed to set 1394B mode. \n"); close(); throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Failed to set 1394B mode")); } } else { if (dc1394_video_set_operation_mode(camera, DC1394_OPERATION_MODE_LEGACY) != DC1394_SUCCESS) { - // vpERROR_TRACE("Cannot set camera to 1394A mode. \n"); close(); throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Cannot set camera to 1394A mode")); } if (dc1394_video_get_operation_mode(camera, &op_mode) != DC1394_SUCCESS) { - // vpERROR_TRACE("Failed to set 1394A mode. \n"); close(); throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Failed to set 1394A mode")); } } if (dc1394_video_set_iso_speed(camera, (dc1394speed_t)isospeed) != DC1394_SUCCESS) { - // vpERROR_TRACE("Cannot set requested iso speed. \n"); close(); throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Cannot set requested iso speed")); } if (dc1394_video_get_iso_speed(camera, &speed) != DC1394_SUCCESS) { - // vpERROR_TRACE("Failed to set iso speed. \n"); close(); throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Failed to set iso speed")); } diff --git a/modules/sensor/src/framegrabber/directshow/vpDirectShowGrabberImpl.cpp b/modules/sensor/src/framegrabber/directshow/vpDirectShowGrabberImpl.cpp index 98d804e540..1657dd1695 100644 --- a/modules/sensor/src/framegrabber/directshow/vpDirectShowGrabberImpl.cpp +++ b/modules/sensor/src/framegrabber/directshow/vpDirectShowGrabberImpl.cpp @@ -500,27 +500,10 @@ bool vpDirectShowGrabberImpl::connectSourceToGrabber(CComPtr &_pCap if (FAILED(hr = pBuild->RenderStream(NULL, NULL, _pCapSource, NULL, _pGrabberFilter))) return false; - /* - //get the grabber's output pin - CComPtr pGrabberOutputPin; - if(FAILED(pBuild->FindPin(_pGrabberFilter, PINDIR_OUTPUT, NULL, NULL, false, - 0, &pGrabberOutputPin))) return false; - */ // get the Null renderer CComPtr pNull = NULL; if (FAILED(pNull.CoCreateInstance(CLSID_NullRenderer, NULL, CLSCTX_INPROC_SERVER))) return false; - /* - //get the null renderer's input pin - CComPtr pNullInputPin; - if(FAILED(pBuild->FindPin(pNull, PINDIR_INPUT, NULL, NULL, false, 0, - &pNullInputPin))) return false; - - //connect the grabber's output to the null renderer - if( FAILED(pGraph->AddFilter(pNull, L"NullRenderer")) || - FAILED(pGraph->Connect(pGrabberOutputPin, pNullInputPin))) - return false; - */ if (FAILED(pGraph->AddFilter(pNull, L"NullRenderer")) || FAILED(pBuild->RenderStream(NULL, NULL, _pGrabberFilter, NULL, pNull))) @@ -538,8 +521,6 @@ bool vpDirectShowGrabberImpl::connectSourceToGrabber(CComPtr &_pCap // release the remaining interfaces pCapSourcePin.Release(); pNull.Release(); - // pGrabberOutputPin.Release(); - // pNullInputPin.Release(); return true; } @@ -803,17 +784,7 @@ bool vpDirectShowGrabberImpl::setFormat(unsigned int width, unsigned int height, VIDEO_STREAM_CONFIG_CAPS scc; AM_MEDIA_TYPE *pmtConfig; hr = pConfig->GetStreamCaps(iFormat, &pmtConfig, (BYTE *)&scc); - // VIDEOINFOHEADER *pVih = - //(VIDEOINFOHEADER*)pmtConfig->pbFormat; - - // pVih->bmiHeader.biWidth; - // pVih->bmiHeader.biHeight; - // 10000000 /pVih->AvgTimePerFrame; - // std::cout<<"available image size : - //"<bmiHeader.biWidth<<" x "<bmiHeader.biHeight<<" at - //"<<10000000 /pVih->AvgTimePerFrame<bmiHeader.biCompression<majortype == sgCB.connectedMediaType.majortype) && @@ -937,12 +908,6 @@ bool vpDirectShowGrabberImpl::getStreamCapabilities() /* Examine the format, and possibly use it. */ VIDEOINFOHEADER *pVih = (VIDEOINFOHEADER *)pmtConfig->pbFormat; - // LONG lWidth = pVih->bmiHeader.biWidth; - // LONG lHeight = - // pVih->bmiHeader.biHeight; SIZE - // dimensions={lWidth,lHeight}; - // LONGLONG lAvgTimePerFrame = - // pVih->AvgTimePerFrame; std::cout << "MediaType : " << iFormat << std::endl; if (pmtConfig->subtype == MEDIASUBTYPE_ARGB32) diff --git a/modules/sensor/src/framegrabber/v4l2/vpV4l2Grabber.cpp b/modules/sensor/src/framegrabber/v4l2/vpV4l2Grabber.cpp index adce5a623f..4047d0122c 100644 --- a/modules/sensor/src/framegrabber/v4l2/vpV4l2Grabber.cpp +++ b/modules/sensor/src/framegrabber/v4l2/vpV4l2Grabber.cpp @@ -1194,12 +1194,6 @@ void vpV4l2Grabber::startStreaming() memcpy(&buf_me[i].fmt, &fmt_me, sizeof(ng_video_fmt)); buf_me[i].size = buf_me[i].fmt.bytesperline * buf_me[i].fmt.height; - // if (m_verbose) - // std::cout << "1: buf_v4l2[" << i << "].length: " << - // buf_v4l2[i].length - // << " buf_v4l2[" << i << "].offset: " << buf_v4l2[i].m.offset - // << std::endl; - buf_me[i].data = (unsigned char *)v4l2_mmap(NULL, buf_v4l2[i].length, PROT_READ | PROT_WRITE, MAP_SHARED, fd, (off_t)buf_v4l2[i].m.offset); @@ -1209,16 +1203,6 @@ void vpV4l2Grabber::startStreaming() buf_me[i].refcount = 0; - // if (m_verbose) - // { - // std::cout << "2: buf_v4l2[" << i << "].length: " << - // buf_v4l2[i].length - // << " buf_v4l2[" << i << "].offset: " << buf_v4l2[i].m.offset - // << std::endl; - // std::cout << "2: buf_me[" << i << "].size: " << buf_me[i].size << - // std::endl; - // } - if (m_verbose) printBufInfo(buf_v4l2[i]); } diff --git a/modules/sensor/src/laserscanner/sick/vpSickLDMRS.cpp b/modules/sensor/src/laserscanner/sick/vpSickLDMRS.cpp index 546d860633..6037e9bd39 100644 --- a/modules/sensor/src/laserscanner/sick/vpSickLDMRS.cpp +++ b/modules/sensor/src/laserscanner/sick/vpSickLDMRS.cpp @@ -234,8 +234,6 @@ bool vpSickLDMRS::measure(vpLaserScan laserscan[4]) // get the start/stop angle short startAngle = (short)ushortptr[12]; short stopAngle = (short)ushortptr[13]; - // std::cout << "angle in [" << startAngle << "; " << stopAngle - // << "]" << std::endl; // get the number of points of this measurement unsigned short numPoints = ushortptr[14]; diff --git a/modules/sensor/src/rgb-depth/kinect/vpKinect.cpp b/modules/sensor/src/rgb-depth/kinect/vpKinect.cpp index 81f7cd684f..79af83887b 100644 --- a/modules/sensor/src/rgb-depth/kinect/vpKinect.cpp +++ b/modules/sensor/src/rgb-depth/kinect/vpKinect.cpp @@ -79,18 +79,14 @@ void vpKinect::start(vpKinect::vpDMResolution res) //! Note that they can differ from one device to another. if (DMres == DMAP_LOW_RES) { std::cout << "vpKinect::start LOW depth map resolution 240x320" << std::endl; - // IRcam.setparameters(IRcam.get_px()/2, IRcam.get_py()/2, - // IRcam.get_u0()/2, IRcam.get_v0()/2); - // IRcam.initPersProjWithoutDistortion(303.06,297.89,160.75,117.9); IRcam.initPersProjWithDistortion(303.06, 297.89, 160.75, 117.9, -0.27, 0); hd = 240; wd = 320; } else { std::cout << "vpKinect::start MEDIUM depth map resolution 480x640" << std::endl; - // IRcam.initPersProjWithoutDistortion(606.12,595.78,321.5,235.8); IRcam.initPersProjWithDistortion(606.12, 595.78, 321.5, 235.8, -0.27, 0); - // Idmap.resize(height, width); + hd = 480; wd = 640; } @@ -124,7 +120,6 @@ void vpKinect::stop() */ void vpKinect::VideoCallback(void *rgb, uint32_t /* timestamp */) { - // std::cout << "vpKinect Video callback" << std::endl; vpMutex::vpScopedLock lock(m_rgb_mutex); uint8_t *rgb_ = static_cast(rgb); for (unsigned i = 0; i < height; i++) { @@ -150,7 +145,6 @@ void vpKinect::VideoCallback(void *rgb, uint32_t /* timestamp */) */ void vpKinect::DepthCallback(void *depth, uint32_t /* timestamp */) { - // std::cout << "vpKinect Depth callback" << std::endl; vpMutex::vpScopedLock lock(m_depth_mutex); uint16_t *depth_ = static_cast(depth); for (unsigned i = 0; i < height; i++) { @@ -185,7 +179,6 @@ bool vpKinect::getDepthMap(vpImage &map) */ bool vpKinect::getDepthMap(vpImage &map, vpImage &Imap) { - // vpMutex::vpScopedLock lock(m_depth_mutex); vpImage tempMap; m_depth_mutex.lock(); if (!m_new_depth_map && !m_new_depth_image) { @@ -255,8 +248,6 @@ void vpKinect::warpRGBFrame(const vpImage &Irgb, const vpImage &I double u = 0., v = 0.; vpColVector P1(4), P2(4); - // std::cout <<"rgbMir : "< &I, unsigned int u, unsigned in n += 1; if (n > nbMaxPoint) { - // vpERROR_TRACE("Too many point %lf (%lf%% of image size). " - // "This threshold can be modified using the - // setMaxDotSize() " "method.", n, n / - //(I.getWidth() * I.getHeight()), nbMaxPoint, - // maxDotSizePercentage) ; - throw(vpTrackingException(vpTrackingException::featureLostError, "Too many point %lf (%lf%% of image size). " "This threshold can be modified using the setMaxDotSize() " @@ -297,8 +289,9 @@ bool vpDot::connexe(const vpImage &I, unsigned int u, unsigned in m20 += u * u; m02 += v * v; } - } else { - // std::cout << "not in" << std::endl; + } + else { + // std::cout << "not in" << std::endl; return false; } @@ -408,35 +401,30 @@ void vpDot::COG(const vpImage &I, double &u, double &v) #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 (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) - { + if (sol == false) { //vpERROR_TRACE("Dot has been lost") ; throw(vpTrackingException(vpTrackingException::featureLostError, - "Dot has been lost")) ; + "Dot has been lost")); } } #else @@ -552,7 +540,8 @@ void vpDot::COG(const vpImage &I, double &u, double &v) if (Ip - (1 - grayLevelPrecision) < 0) { gray_level_min = 0; - } else { + } + else { gray_level_min = (unsigned int)(255 * pow(Ip - (1 - grayLevelPrecision), gamma)); if (gray_level_min > 255) gray_level_min = 255; @@ -570,12 +559,6 @@ void vpDot::COG(const vpImage &I, double &u, double &v) } if (npoint > nbMaxPoint) { - // vpERROR_TRACE("Too many point %lf (%lf%%). Max allowed is %lf - // (%lf%%). This threshold can be modified using the setMaxDotSize() - // method.", - // npoint, npoint / (I.getWidth() * I.getHeight()), - // nbMaxPoint, maxDotSizePercentage) ; - throw(vpTrackingException(vpTrackingException::featureLostError, "Too many point %lf (%lf%%). Max allowed is " "%lf (%lf%%). This threshold can be modified " @@ -602,7 +585,8 @@ void vpDot::setMaxDotSize(double percentage) // 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); - } else { + } + else { maxDotSizePercentage = percentage; } } @@ -642,7 +626,8 @@ void vpDot::initTracking(const vpImage &I) if (Ip - (1 - grayLevelPrecision) < 0) { gray_level_min = 0; - } else { + } + else { gray_level_min = (unsigned int)(255 * pow(Ip - (1 - grayLevelPrecision), gamma)); if (gray_level_min > 255) gray_level_min = 255; @@ -653,7 +638,8 @@ void vpDot::initTracking(const vpImage &I) try { track(I); - } catch (const vpException &e) { + } + catch (const vpException &e) { throw(e); } } @@ -693,7 +679,8 @@ void vpDot::initTracking(const vpImage &I, const vpImagePoint &ip if (Ip - (1 - grayLevelPrecision) < 0) { gray_level_min = 0; - } else { + } + else { gray_level_min = (unsigned int)(255 * pow(Ip - (1 - grayLevelPrecision), gamma)); if (gray_level_min > 255) gray_level_min = 255; @@ -703,7 +690,8 @@ void vpDot::initTracking(const vpImage &I, const vpImagePoint &ip gray_level_max = 255; try { track(I); - } catch (const vpException &e) { + } + catch (const vpException &e) { throw(e); } } @@ -746,7 +734,8 @@ void vpDot::initTracking(const vpImage &I, const vpImagePoint &ip try { track(I); - } catch (const vpException &e) { + } + catch (const vpException &e) { throw(e); } } @@ -788,7 +777,8 @@ void vpDot::track(const vpImage &I) vpDisplay::displayCross(I, this->cog, 3 * thickness + 8, vpColor::red, thickness); } - } catch (const vpException &e) { + } + catch (const vpException &e) { throw(e); } } @@ -854,9 +844,11 @@ void vpDot::setGrayLevelPrecision(const double &precision) double epsilon = 0.05; if (grayLevelPrecision < epsilon) { this->grayLevelPrecision = epsilon; - } else if (grayLevelPrecision > 1) { + } + else if (grayLevelPrecision > 1) { this->grayLevelPrecision = 1.0; - } else { + } + else { this->grayLevelPrecision = precision; } } diff --git a/modules/tracker/blob/src/dots/vpDot2.cpp b/modules/tracker/blob/src/dots/vpDot2.cpp index b63bef9787..4c2d5607be 100644 --- a/modules/tracker/blob/src/dots/vpDot2.cpp +++ b/modules/tracker/blob/src/dots/vpDot2.cpp @@ -528,24 +528,6 @@ void vpDot2::track(const vpImage &I, bool canMakeTheWindowGrow) bbox_v_min = movingDot.bbox_v_min; bbox_v_max = movingDot.bbox_v_max; } - // else { - // // test if the found dot is valid, - // if( ! isValid( I, wantedDot ) ) { - // *this = wantedDot; - // vpERROR_TRACE("The found dot is invalid:", - // "- could be a problem of size (width or height) or " - // " surface (number of pixels) which differ too much " - // " to the previous one " - // "- or a problem of the shape which is not ellipsoid if " - // " use setEllipsoidShapePrecision(double - // ellipsoidShapePrecision) " - // " which is the default case. " - // " To track a non ellipsoid shape use - // setEllipsoidShapePrecision(0)") ; - // throw(vpTrackingException(vpTrackingException::featureLostError, - // "The found dot is invalid")) ; - // } - // } // if this dot is partially out of the image, return an error tracking. if (!isInImage(I)) { @@ -1358,8 +1340,6 @@ bool vpDot2::isValid(const vpImage &I, const vpDot2 &wantedDot) u = (unsigned int)(cog_u + innerCoef * (a1 * cos(alpha) * cos(theta) - a2 * sin(alpha) * sin(theta))); v = (unsigned int)(cog_v + innerCoef * (a1 * sin(alpha) * cos(theta) + a2 * cos(alpha) * sin(theta))); if (!this->hasGoodLevel(I, u, v)) { -// vpTRACE("Inner circle pixel (%d, %d) has bad level for dot (%g, %g)", -// u, v, cog_u, cog_v); #ifdef DEBUG printf("Inner circle pixel (%u, %u) has bad level for dot (%g, %g): " "%d not in [%u, %u]\n", @@ -1407,8 +1387,6 @@ bool vpDot2::isValid(const vpImage &I, const vpDot2 &wantedDot) continue; } if (!this->hasReverseLevel(I, u, v)) { -// vpTRACE("Outside circle pixel (%d, %d) has bad level for dot (%g, -// %g)", u, v, cog_u, cog_v); #ifdef DEBUG printf("Outside circle pixel (%u, %u) has bad level for dot (%g, " "%g): %d not in [%u, %u]\n", @@ -1629,10 +1607,6 @@ bool vpDot2::computeParameters(const vpImage &I, const double &_u int border_u = (int)this->firstBorder_u; int border_v = (int)this->firstBorder_v; - - // vpTRACE("-----------------------------------------"); - // vpTRACE("first border_u: %d border_v: %d dir: %d", - // this->firstBorder_u, this->firstBorder_v,firstDir); int du, dv; float dS, dMu, dMv, dMuv, dMu2, dMv2; m00 = 0.0; @@ -1741,14 +1715,6 @@ bool vpDot2::computeParameters(const vpImage &I, const double &_u mu20 = m20 - tmpCenter_u * m10; } - // check the center is in the image... never know... - // if( !hasGoodLevel( I, (unsigned int)tmpCenter_u, - // (unsigned int)tmpCenter_v ) ) - // { - // vpDEBUG_TRACE(3, "The center of gravity of the dot (%g, %g) has - // not a good in level", tmpCenter_u, tmpCenter_v); return false; - // } - cog.set_u(tmpCenter_u); cog.set_v(tmpCenter_v); } diff --git a/modules/tracker/dnn/src/vpMegaPose.cpp b/modules/tracker/dnn/src/vpMegaPose.cpp index 629e8ebfca..e54af3f485 100644 --- a/modules/tracker/dnn/src/vpMegaPose.cpp +++ b/modules/tracker/dnn/src/vpMegaPose.cpp @@ -50,7 +50,7 @@ #include #include #include -using json = nlohmann::json; +using json = nlohmann::json; //! json namespace shortcut //// Network message utils diff --git a/modules/tracker/klt/include/visp3/klt/vpKltOpencv.h b/modules/tracker/klt/include/visp3/klt/vpKltOpencv.h index 0f619eb8ef..edce98a3a0 100644 --- a/modules/tracker/klt/include/visp3/klt/vpKltOpencv.h +++ b/modules/tracker/klt/include/visp3/klt/vpKltOpencv.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -31,8 +30,7 @@ * Description: * Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented * with opencv. - * -*****************************************************************************/ + */ /*! \file vpKltOpencv.h @@ -55,47 +53,130 @@ #include /*! - \class vpKltOpencv - - \ingroup module_klt - - \brief Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker - implemented in OpenCV. Thus to enable this class OpenCV should be installed. - Installation instructions are provided here - https://visp.inria.fr/3rd_opencv. - - The following example available in tutorial-klt-tracker.cpp shows how to use - the main functions of the class. - - \include tutorial-klt-tracker.cpp - - A line by line explanation is provided in \ref tutorial-tracking-keypoint. -*/ + * \class vpKltOpencv + * + * \ingroup module_klt + * + * \brief Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker + * implemented in OpenCV. Thus to enable this class OpenCV should be installed. + * Installation instructions are provided here + * https://visp.inria.fr/3rd_opencv. + * + * The following example available in tutorial-klt-tracker.cpp shows how to use + * the main functions of the class. + * + * \include tutorial-klt-tracker.cpp + * + * A line by line explanation is provided in \ref tutorial-tracking-keypoint. + */ class VISP_EXPORT vpKltOpencv { public: + /*! + * Default constructor. + */ vpKltOpencv(); + /*! + * Copy constructor. + */ vpKltOpencv(const vpKltOpencv ©); + /*! + * Destructor. + */ virtual ~vpKltOpencv(); + /*! + * Add a keypoint at the end of the feature list. The id of the feature is set + * to ensure that it is unique. \param x,y : Coordinates of the feature in the + * image. + */ void addFeature(const float &x, const float &y); + + /*! + * Add a keypoint at the end of the feature list. + * + * \warning This function doesn't ensure that the id of the feature is unique. + * You should rather use addFeature(const float &, const float &) or + * addFeature(const cv::Point2f &). + * + * \param id : Feature id. Should be unique + * \param x,y : Coordinates of the feature in the image. + */ void addFeature(const long &id, const float &x, const float &y); + + /*! + * Add a keypoint at the end of the feature list. The id of the feature is set + * to ensure that it is unique. + * \param f : Coordinates of the feature in the image. + */ void addFeature(const cv::Point2f &f); + /*! + * Display features position and id. + * + * \param I : Image used as background. Display should be initialized on it. + * \param color : Color used to display the features. + * \param thickness : Thickness of the drawings. + */ void display(const vpImage &I, const vpColor &color = vpColor::red, unsigned int thickness = 1); + /*! + * Display features list. + * + * \param I : The image used as background. + * \param features : Vector of features. + * \param color : Color used to display the points. + * \param thickness : Thickness of the points. + */ static void display(const vpImage &I, const std::vector &features, const vpColor &color = vpColor::green, unsigned int thickness = 1); + /*! + * Display features list. + * + * \param I : The image used as background. + * \param features : Vector of features. + * \param color : Color used to display the points. + * \param thickness : Thickness of the points. + */ static void display(const vpImage &I, const std::vector &features, const vpColor &color = vpColor::green, unsigned int thickness = 1); + /*! + * Display features list with ids. + * + * \param I : The image used as background. + * \param features : Vector of features. + * \param featuresid : Vector of ids corresponding to the features. + * \param color : Color used to display the points. + * \param thickness : Thickness of the points + */ static void display(const vpImage &I, const std::vector &features, - const std::vector &featuresid, const vpColor &color = vpColor::green, - unsigned int thickness = 1); + const std::vector &featuresid, const vpColor &color = vpColor::green, + unsigned int thickness = 1); + /*! + * Display features list with ids. + * + * \param I : The image used as background. + * \param features : Vector of features. + * \param featuresid : Vector of ids corresponding to the features. + * \param color : Color used to display the points. + * \param thickness : Thickness of the points + */ static void display(const vpImage &I, const std::vector &features, const std::vector &featuresid, const vpColor &color = vpColor::green, unsigned int thickness = 1); //! Get the size of the averaging block used to track the features. int getBlockSize() const { return m_blockSize; } + /*! + * Get the 'index'th feature image coordinates. Beware that + * getFeature(i,...) may not represent the same feature before and + * after a tracking iteration (if a feature is lost, features are + * shifted in the array). + * + * \param index : Index of feature. + * \param id : id of the feature. + * \param x : x coordinate. + * \param y : y coordinate. + */ void getFeature(const int &index, long &id, float &x, float &y) const; //! Get the list of current features. std::vector getFeatures() const { return m_points[1]; } @@ -130,45 +211,194 @@ class VISP_EXPORT vpKltOpencv //! Get the window size used to refine the corner locations. int getWindowSize() const { return m_winSize; } + /*! + * Initialise the tracking by extracting KLT keypoints on the provided image. + * + * \param I : Grey level image used as input. This image should have only 1 channel. + * \param mask : Image mask used to restrict the keypoint detection + * area. If mask is NULL, all the image will be considered. + * + * \exception vpTrackingException::initializationError : If the image I is not + * initialized, or if the image or the mask have bad coding format. + */ void initTracking(const cv::Mat &I, const cv::Mat &mask = cv::Mat()); + + /*! + * Set the points that will be used as initialization during the next call to + * track(). + * + * \param I : Input image. + * \param pts : Vector of points that should be tracked. + */ void initTracking(const cv::Mat &I, const std::vector &pts); + + /*! + * Set the points that will be used as initialization during the next call to + * track(). + * + * \param I : Input image. + * \param pts : Vector of points that should be tracked. + * \param ids : Corresponding point ids. + */ void initTracking(const cv::Mat &I, const std::vector &pts, const std::vector &ids); + /*! + * Copy operator. + */ vpKltOpencv &operator=(const vpKltOpencv ©); + + /*! + * Track KLT keypoints using the iterative Lucas-Kanade method with pyramids. + * + * \param I : Input image. + */ void track(const cv::Mat &I); - void setBlockSize(int blockSize); - void setHarrisFreeParameter(double harris_k); + + /*! + * Set the size of the averaging block used to track the features. + * + * \warning The input is a signed integer to be compatible with OpenCV. + * However, it must be a positive integer. + * + * \param blockSize : Size of an average block for computing a derivative + * covariation matrix over each pixel neighborhood. Default value is set to 3. + */ + void setBlockSize(int blockSize) { m_blockSize = blockSize; } + + /*! + * Set the free parameter of the Harris detector. + * + * \param harris_k : Free parameter of the Harris detector. Default value is + * set to 0.04. + */ + void setHarrisFreeParameter(double harris_k) { m_harris_k = harris_k; } + + /*! + * Set the points that will be used as initial guess during the next call to + * track(). A typical usage of this function is to predict the position of the + * features before the next call to track(). + * + * \param guess_pts : Vector of points that should be tracked. The size of this + * vector should be the same as the one returned by getFeatures(). If this is + * not the case, an exception is returned. Note also that the id of the points + * is not modified. + * + * \sa initTracking() + */ void setInitialGuess(const std::vector &guess_pts); + + /*! + * Set the points that will be used as initial guess during the next call to + * track(). A typical usage of this function is to predict the position of the + * features before the next call to track(). + * + * \param init_pts : Initial points (could be obtained from getPrevFeatures() + * or getFeatures()). + * \param guess_pts : Prediction of the new position of the + * initial points. The size of this vector must be the same as the size of the + * vector of initial points. + * \param fid : Identifiers of the initial points. + * + * \sa getPrevFeatures(),getPrevFeaturesId + * \sa getFeatures(), getFeaturesId + * \sa initTracking() + */ void setInitialGuess(const std::vector &init_pts, const std::vector &guess_pts, const std::vector &fid); - void setMaxFeatures(int maxCount); - void setMinDistance(double minDistance); - void setMinEigThreshold(double minEigThreshold); - void setPyramidLevels(int pyrMaxLevel); - void setQuality(double qualityLevel); + /*! + * Set the maximum number of features to track in the image. + * + * \param maxCount : Maximum number of features to detect and track. Default + * value is set to 500. + */ + void setMaxFeatures(int maxCount) { m_maxCount = maxCount; } + + /*! + * Set the minimal Euclidean distance between detected corners during + * initialization. + * + * \param minDistance : Minimal possible Euclidean distance between the + * detected corners. Default value is set to 15. + */ + void setMinDistance(double minDistance) { m_minDistance = minDistance; } + + /*! + * Set the minimal eigen value threshold used to reject a point during the + * tracking. + * + * \param minEigThreshold : Minimal eigen value threshold. Default + * value is set to 1e-4. + */ + void setMinEigThreshold(double minEigThreshold) { m_minEigThreshold = minEigThreshold; } + + /*! + * Set the maximal pyramid level. If the level is zero, then no pyramid is + * computed for the optical flow. + * + * \param pyrMaxLevel : 0-based maximal pyramid level number; if set to 0, + * pyramids are not used (single level), if set to 1, two levels are used, and + * so on. Default value is set to 3. + */ + void setPyramidLevels(int pyrMaxLevel) { m_pyrMaxLevel = pyrMaxLevel; } + + /*! + * Set the parameter characterizing the minimal accepted quality of image + * corners. + * + * \param qualityLevel : Quality level parameter. Default value is set to 0.01. + * The parameter value is multiplied by the best corner quality measure, which + * is the minimal eigenvalue or the Harris function response. The corners with + * the quality measure less than the product are rejected. For example, if the + * best corner has the quality measure = 1500, and the qualityLevel=0.01, then + * all the corners with the quality measure less than 15 are rejected. + */ + void setQuality(double qualityLevel) { m_qualityLevel = qualityLevel; } + //! Does nothing. Just here for compat with previous releases that use //! OpenCV C api to do the tracking. void setTrackerId(int tid) { (void)tid; } - void setUseHarris(int useHarrisDetector); - void setWindowSize(int winSize); + + /*! + * Set the parameter indicating whether to use a Harris detector or + * the minimal eigenvalue of gradient matrices for corner detection. + * \param useHarrisDetector : If 1 (default value), use the Harris detector. If + * 0 use the eigenvalue. + */ + void setUseHarris(int useHarrisDetector) { m_useHarrisDetector = useHarrisDetector; } + + /*! + * Set the window size used to refine the corner locations. + * + * \param winSize : Half of the side length of the search window. Default value + * is set to 10. For example, if \e winSize=5 , then a 5*2+1 \f$\times\f$ 5*2+1 + * = 11 \f$\times\f$ 11 search window is used. + */ + void setWindowSize(int winSize) { m_winSize = winSize; } + + /*! + * Remove the feature with the given index as parameter. + * + * \param index : Index of the feature to remove. + */ void suppressFeature(const int &index); protected: - cv::Mat m_gray, m_prevGray; + cv::Mat m_gray; //!< Gray image + cv::Mat m_prevGray; //!< Previous gray image std::vector m_points[2]; //!< Previous [0] and current [1] keypoint location std::vector m_points_id; //!< Keypoint id - int m_maxCount; - cv::TermCriteria m_termcrit; - int m_winSize; - double m_qualityLevel; - double m_minDistance; - double m_minEigThreshold; - double m_harris_k; - int m_blockSize; - int m_useHarrisDetector; - int m_pyrMaxLevel; - long m_next_points_id; - bool m_initial_guess; + int m_maxCount; //!< Max number of keypoints + cv::TermCriteria m_termcrit; //!< Term criteria + int m_winSize; //!< Window criteria + double m_qualityLevel; //!< Quality level + double m_minDistance; //!< Mins distance between keypoints + double m_minEigThreshold; //!< Min eigen threshold + double m_harris_k; //!< Harris parameter + int m_blockSize; //!< Block size + int m_useHarrisDetector; //!< true to use Harris detector + int m_pyrMaxLevel; //!< Pyramid max level + long m_next_points_id; //!< Id for the newt keypoint + bool m_initial_guess; //!< true when initial guess is provided }; #endif diff --git a/modules/tracker/klt/src/vpKltOpencv.cpp b/modules/tracker/klt/src/vpKltOpencv.cpp index 985bef5b37..ffd237b816 100644 --- a/modules/tracker/klt/src/vpKltOpencv.cpp +++ b/modules/tracker/klt/src/vpKltOpencv.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -31,8 +30,7 @@ * Description: * Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented * with opencv. - * -*****************************************************************************/ + */ /*! \file vpKltOpencv.cpp @@ -51,9 +49,6 @@ #include #include -/*! - Default constructor. - */ vpKltOpencv::vpKltOpencv() : m_gray(), m_prevGray(), m_points_id(), m_maxCount(500), m_termcrit(), m_winSize(10), m_qualityLevel(0.01), m_minDistance(15), m_minEigThreshold(1e-4), m_harris_k(0.04), m_blockSize(3), m_useHarrisDetector(1), @@ -62,9 +57,6 @@ vpKltOpencv::vpKltOpencv() m_termcrit = cv::TermCriteria(cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 20, 0.03); } -/*! - Copy constructor. - */ vpKltOpencv::vpKltOpencv(const vpKltOpencv ©) : m_gray(), m_prevGray(), m_points_id(), m_maxCount(500), m_termcrit(), m_winSize(10), m_qualityLevel(0.01), m_minDistance(15), m_minEigThreshold(1e-4), m_harris_k(0.04), m_blockSize(3), m_useHarrisDetector(1), @@ -73,9 +65,6 @@ vpKltOpencv::vpKltOpencv(const vpKltOpencv ©) *this = copy; } -/*! - Copy operator. - */ vpKltOpencv &vpKltOpencv::operator=(const vpKltOpencv ©) { m_gray = copy.m_gray; @@ -101,16 +90,6 @@ vpKltOpencv &vpKltOpencv::operator=(const vpKltOpencv ©) vpKltOpencv::~vpKltOpencv() {} -/*! - Initialise the tracking by extracting KLT keypoints on the provided image. - - \param I : Grey level image used as input. This image should have only 1 channel. - \param mask : Image mask used to restrict the keypoint detection - area. If mask is NULL, all the image will be considered. - - \exception vpTrackingException::initializationError : If the image I is not - initialized, or if the image or the mask have bad coding format. -*/ void vpKltOpencv::initTracking(const cv::Mat &I, const cv::Mat &mask) { m_next_points_id = 0; @@ -135,11 +114,6 @@ void vpKltOpencv::initTracking(const cv::Mat &I, const cv::Mat &mask) } } -/*! - Track KLT keypoints using the iterative Lucas-Kanade method with pyramids. - - \param I : Input image. - */ void vpKltOpencv::track(const cv::Mat &I) { if (m_points[1].size() == 0) @@ -179,19 +153,6 @@ void vpKltOpencv::track(const cv::Mat &I) } } -/*! - - Get the 'index'th feature image coordinates. Beware that - getFeature(i,...) may not represent the same feature before and - after a tracking iteration (if a feature is lost, features are - shifted in the array). - - \param index : Index of feature. - \param id : id of the feature. - \param x : x coordinate. - \param y : y coordinate. - -*/ void vpKltOpencv::getFeature(const int &index, long &id, float &x, float &y) const { if ((size_t)index >= m_points[1].size()) { @@ -203,30 +164,11 @@ void vpKltOpencv::getFeature(const int &index, long &id, float &x, float &y) con id = m_points_id[(size_t)index]; } -/*! - Display features position and id. - - \param I : Image used as background. Display should be initialized on it. - \param color : Color used to display the features. - \param thickness : Thickness of the drawings. - */ void vpKltOpencv::display(const vpImage &I, const vpColor &color, unsigned int thickness) { vpKltOpencv::display(I, m_points[1], m_points_id, color, thickness); } -/*! - - Display features list. - - \param I : The image used as background. - - \param features : Vector of features. - - \param color : Color used to display the points. - - \param thickness : Thickness of the points. -*/ void vpKltOpencv::display(const vpImage &I, const std::vector &features, const vpColor &color, unsigned int thickness) { @@ -238,18 +180,6 @@ void vpKltOpencv::display(const vpImage &I, const std::vector &I, const std::vector &features, const vpColor &color, unsigned int thickness) { @@ -261,20 +191,6 @@ void vpKltOpencv::display(const vpImage &I, const std::vector &I, const std::vector &features, const std::vector &featuresid, const vpColor &color, unsigned int thickness) { @@ -291,20 +207,6 @@ void vpKltOpencv::display(const vpImage &I, const std::vector &I, const std::vector &features, const std::vector &featuresid, const vpColor &color, unsigned int thickness) { @@ -321,101 +223,6 @@ void vpKltOpencv::display(const vpImage &I, const std::vector &guess_pts) { if (guess_pts.size() != m_points[1].size()) { @@ -429,20 +236,6 @@ void vpKltOpencv::setInitialGuess(const std::vector &guess_pts) m_initial_guess = true; } -/*! - Set the points that will be used as initial guess during the next call to - track(). A typical usage of this function is to predict the position of the - features before the next call to track(). - - \param init_pts : Initial points (could be obtained from getPrevFeatures() - or getFeatures()). \param guess_pts : Prediction of the new position of the - initial points. The size of this vector must be the same as the size of the - vector of initial points. \param fid : Identifiers of the initial points. - - \sa getPrevFeatures(),getPrevFeaturesId - \sa getFeatures(), getFeaturesId - \sa initTracking() -*/ void vpKltOpencv::setInitialGuess(const std::vector &init_pts, const std::vector &guess_pts, const std::vector &fid) { @@ -459,14 +252,6 @@ void vpKltOpencv::setInitialGuess(const std::vector &init_pts, cons m_initial_guess = true; } -/*! - Set the points that will be used as initialization during the next call to - track(). - - \param I : Input image. - \param pts : Vector of points that should be tracked. - -*/ void vpKltOpencv::initTracking(const cv::Mat &I, const std::vector &pts) { m_initial_guess = false; @@ -503,13 +288,6 @@ void vpKltOpencv::initTracking(const cv::Mat &I, const std::vector I.copyTo(m_gray); } -/*! - - Add a keypoint at the end of the feature list. The id of the feature is set - to ensure that it is unique. \param x,y : Coordinates of the feature in the - image. - -*/ void vpKltOpencv::addFeature(const float &x, const float &y) { cv::Point2f f(x, y); @@ -517,18 +295,6 @@ void vpKltOpencv::addFeature(const float &x, const float &y) m_points_id.push_back(m_next_points_id++); } -/*! - - Add a keypoint at the end of the feature list. - - \warning This function doesn't ensure that the id of the feature is unique. - You should rather use addFeature(const float &, const float &) or - addFeature(const cv::Point2f &). - - \param id : Feature id. Should be unique - \param x,y : Coordinates of the feature in the image. - -*/ void vpKltOpencv::addFeature(const long &id, const float &x, const float &y) { cv::Point2f f(x, y); @@ -538,23 +304,12 @@ void vpKltOpencv::addFeature(const long &id, const float &x, const float &y) m_next_points_id = id + 1; } -/*! - - Add a keypoint at the end of the feature list. The id of the feature is set - to ensure that it is unique. \param f : Coordinates of the feature in the - image. - -*/ void vpKltOpencv::addFeature(const cv::Point2f &f) { m_points[1].push_back(f); m_points_id.push_back(m_next_points_id++); } -/*! - Remove the feature with the given index as parameter. - \param index : Index of the feature to remove. - */ void vpKltOpencv::suppressFeature(const int &index) { if ((size_t)index >= m_points[1].size()) { diff --git a/modules/tracker/mbt/src/edge/vpMbtDistanceCylinder.cpp b/modules/tracker/mbt/src/edge/vpMbtDistanceCylinder.cpp index ed10b47be0..1a0d03810b 100644 --- a/modules/tracker/mbt/src/edge/vpMbtDistanceCylinder.cpp +++ b/modules/tracker/mbt/src/edge/vpMbtDistanceCylinder.cpp @@ -72,8 +72,6 @@ vpMbtDistanceCylinder::vpMbtDistanceCylinder() */ vpMbtDistanceCylinder::~vpMbtDistanceCylinder() { - // cout << "Deleting cylinder " << index << endl; - if (p1 != NULL) delete p1; if (p2 != NULL) diff --git a/modules/tracker/mbt/src/edge/vpMbtDistanceLine.cpp b/modules/tracker/mbt/src/edge/vpMbtDistanceLine.cpp index 6533f03e2b..80083734bc 100644 --- a/modules/tracker/mbt/src/edge/vpMbtDistanceLine.cpp +++ b/modules/tracker/mbt/src/edge/vpMbtDistanceLine.cpp @@ -66,7 +66,6 @@ vpMbtDistanceLine::vpMbtDistanceLine() */ vpMbtDistanceLine::~vpMbtDistanceLine() { - // cout << "Deleting line " << index << endl; if (line != NULL) delete line; diff --git a/modules/tracker/mbt/src/vpMbGenericTracker.cpp b/modules/tracker/mbt/src/vpMbGenericTracker.cpp index 4b0f45a897..47b340b887 100644 --- a/modules/tracker/mbt/src/vpMbGenericTracker.cpp +++ b/modules/tracker/mbt/src/vpMbGenericTracker.cpp @@ -43,7 +43,7 @@ #ifdef VISP_HAVE_NLOHMANN_JSON #include -using json = nlohmann::json; +using json = nlohmann::json; //! json namespace shortcut #endif vpMbGenericTracker::vpMbGenericTracker() @@ -2223,7 +2223,7 @@ void vpMbGenericTracker::initFaceFromLines(vpMbtPolygon & /*polygon*/) 4 # Number of image points in the file (has to be the same as the number of 3D points) 100 200 # \ ... # | 2D coordinates in pixel in the image - 50 10 # / + 50 10 # / \endcode \param I1 : Input grayscale image for the first camera. @@ -2284,7 +2284,7 @@ void vpMbGenericTracker::initFromPoints(const vpImage &I1, const 4 # Number of image points in the file (has to be the same as the number of 3D points) 100 200 # \ ... # | 2D coordinates in pixel in the image - 50 10 # / + 50 10 # / \endcode \param I_color1 : Input color image for the first camera. diff --git a/modules/tracker/mbt/src/vpMbTracker.cpp b/modules/tracker/mbt/src/vpMbTracker.cpp index 881adbe8df..68b7132512 100644 --- a/modules/tracker/mbt/src/vpMbTracker.cpp +++ b/modules/tracker/mbt/src/vpMbTracker.cpp @@ -953,7 +953,7 @@ void vpMbTracker::initFromPoints(const vpImage *const I, const vp as the number of 3D points) 100 200 # \ ... # | 2D coordinates in pixel in the image - 50 10 # / + 50 10 # / \endcode \param I : Input grayscale image @@ -982,7 +982,7 @@ void vpMbTracker::initFromPoints(const vpImage &I, const std::str as the number of 3D points) 100 200 # \ ... # | 2D coordinates in pixel in the image - 50 10 # / + 50 10 # / \endcode \param I_color : Input color image diff --git a/modules/tracker/mbt/test/generic-with-dataset/testMbtJsonSettings.cpp b/modules/tracker/mbt/test/generic-with-dataset/testMbtJsonSettings.cpp index 0a2a64922f..a609e5d014 100644 --- a/modules/tracker/mbt/test/generic-with-dataset/testMbtJsonSettings.cpp +++ b/modules/tracker/mbt/test/generic-with-dataset/testMbtJsonSettings.cpp @@ -44,7 +44,7 @@ #if defined(VISP_HAVE_NLOHMANN_JSON) && defined(VISP_HAVE_CATCH2) #include -using json = nlohmann::json; +using json = nlohmann::json; //! json namespace shortcut #define CATCH_CONFIG_RUNNER #include diff --git a/modules/tracker/me/include/visp3/me/vpMe.h b/modules/tracker/me/include/visp3/me/vpMe.h index dfd501e07a..4f89335181 100644 --- a/modules/tracker/me/include/visp3/me/vpMe.h +++ b/modules/tracker/me/include/visp3/me/vpMe.h @@ -114,16 +114,22 @@ The content of the `me.json` file is the following: \code{.unparsed} $ cat me.json - {"maskSign":0,"maskSize":5,"minSampleStep":4.0,"mu":[0.5,0.5],"nMask":180,"ntotalSample":0,"pointsToTrack":200,"range":5,"sampleStep":10.0,"strip":2,"threshold":20.0,"thresholdType":1} + {"maskSign":0,"maskSize":5,"minSampleStep":4.0,"mu":[0.5,0.5],"nMask":180,"ntotalSample":0,"pointsToTrack":200, + "range":5,"sampleStep":10.0,"strip":2,"threshold":20.0,"thresholdType":1} \endcode */ class VISP_EXPORT vpMe { public: + /*! + * Type of likelihood threshold to use. + */ typedef enum { - OLD_THRESHOLD = 0, /*!< Old likelihood ratio threshold (to be avoided). */ - NORMALIZED_THRESHOLD = 1, /*!< Easy-to-use normalized likelihood threshold corresponding to the minimal luminance contrast to consider with values in [0 ; 255]. */ + //! Old likelihood ratio threshold (to be avoided). + OLD_THRESHOLD = 0, + //! Easy-to-use normalized likelihood threshold corresponding to the minimal luminance contrast to consider with values in [0 ; 255]. + NORMALIZED_THRESHOLD = 1 } vpLikelihoodThresholdType; private: @@ -160,19 +166,39 @@ class VISP_EXPORT vpMe vpMatrix *mask; //! Array of matrices defining the different masks (one for every angle step). public: + /*! + * Default constructor. + */ vpMe(); + /*! + * Copy constructor. + */ vpMe(const vpMe &me); + /*! + * Destructor. + */ virtual ~vpMe(); + /*! + * Copy operator. + */ vpMe &operator=(const vpMe &me); #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) + /*! + * Move operator. + */ vpMe &operator=(const vpMe &&me); #endif - void checkSamplestep(double &a) + /*! + * Check sample step wrt min value. + * \param[inout] sample_step : When this value is lower than the min sample step value, + * it is modified to the min sample step value. + */ + void checkSamplestep(double &sample_step) { - if (a < min_samplestep) - a = min_samplestep; + if (sample_step < min_samplestep) + sample_step = min_samplestep; } /*! Return the angle step. @@ -270,7 +296,15 @@ class VISP_EXPORT vpMe */ inline vpLikelihoodThresholdType getLikelihoodThresholdType() const { return m_likelihood_threshold_type; } + /*! + Initialise the array of matrices with the defined size and the number of + matrices to create. + */ void initMask(); // convolution masks - offset computation + + /*! + * Print using std::cout moving edges settings. + */ void print(); /*! @@ -411,19 +445,63 @@ class VISP_EXPORT vpMe void setLikelihoodThresholdType(const vpLikelihoodThresholdType likelihood_threshold_type) { m_likelihood_threshold_type = likelihood_threshold_type; } #ifdef VISP_HAVE_NLOHMANN_JSON + /*! + * @brief Convert a vpMe object to a JSON representation + * + * @param j resulting json object + * @param me the object to convert + */ friend void to_json(nlohmann::json &j, const vpMe &me); + + /** + * @brief Retrieve a vpMe object from a JSON representation + * + * JSON content (key: type): + * - thresholdType: int, vpMe::getLikelihoodThresholdType() + * - threshold: double, vpMe::setThreshold() + * - mu : [double, double], vpMe::setMu1, vpMe::setMu2() + * - minSampleStep: double, vpMe::setMinSampleStep() + * - angleStep: double, vpMe::setAngleStep() + * - sampleStep: double, vpMe::setSampleStep() + * - range: int, vpMe::setRange() + * - ntotal_sample: int, vpMe::setNbTotalSample() + * - pointsToTrack: int, vpMe::setPointsToTrack() + * - maskSize: int, vpMe::setMaskSize() + * - nMask: int, vpMe::setMaskNumber() + * - maskSign: int, vpMe::setMaskSign() + * - strip: int, vpMe::setStrip() + * + * Example: + * \code{.json} + * { + * "angleStep": 1, + * "maskSign": 0, + * "maskSize": 5, + * "minSampleStep": 4.0, + * "mu": [ + * 0.5, + * 0.5 + * ], + * "nMask": 180, + * "ntotal_sample": 0, + * "pointsToTrack": 500, + * "range": 7, + * "sampleStep": 4.0, + * "strip": 2, + * "thresholdType": 1 + * "threshold": 20.0 + * } + * \endcode + * + * @param j JSON representation to convert + * @param me converted object + */ friend void from_json(const nlohmann::json &j, vpMe &me); #endif }; #ifdef VISP_HAVE_NLOHMANN_JSON #include -/** - * @brief Convert a vpMe object to a JSON representation - * - * @param j resulting json object - * @param me the object to convert - */ inline void to_json(nlohmann::json &j, const vpMe &me) { j = { @@ -442,49 +520,6 @@ inline void to_json(nlohmann::json &j, const vpMe &me) }; } -/** - * @brief Retrieve a vpMe object from a JSON representation - * - * JSON content (key: type): - * - thresholdType: int, vpMe::getLikelihoodThresholdType() - * - threshold: double, vpMe::setThreshold() - * - mu : [double, double], vpMe::setMu1, vpMe::setMu2() - * - minSampleStep: double, vpMe::setMinSampleStep() - * - angleStep: double, vpMe::setAngleStep() - * - sampleStep: double, vpMe::setSampleStep() - * - range: int, vpMe::setRange() - * - ntotal_sample: int, vpMe::setNbTotalSample() - * - pointsToTrack: int, vpMe::setPointsToTrack() - * - maskSize: int, vpMe::setMaskSize() - * - nMask: int, vpMe::setMaskNumber() - * - maskSign: int, vpMe::setMaskSign() - * - strip: int, vpMe::setStrip() - * - * Example: - * \code{.json} - * { - * "angleStep": 1, - "maskSign": 0, - "maskSize": 5, - "minSampleStep": 4.0, - "mu": [ - 0.5, - 0.5 - ], - "nMask": 180, - "ntotal_sample": 0, - "pointsToTrack": 500, - "range": 7, - "sampleStep": 4.0, - "strip": 2, - "thresholdType": 1 - "threshold": 20.0 - } - * \endcode - * - * @param j JSON representation to convert - * @param me converted object - */ inline void from_json(const nlohmann::json &j, vpMe &me) { if (j.contains("thresholdType")) { diff --git a/modules/tracker/me/include/visp3/me/vpMeEllipse.h b/modules/tracker/me/include/visp3/me/vpMeEllipse.h index b26253c520..a3223856e0 100644 --- a/modules/tracker/me/include/visp3/me/vpMeEllipse.h +++ b/modules/tracker/me/include/visp3/me/vpMeEllipse.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -27,8 +26,7 @@ * * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - * -*****************************************************************************/ + */ /*! \file vpMeEllipse.h @@ -91,21 +89,39 @@ class VISP_EXPORT vpMeEllipse : public vpMeTracker { public: + /*! + * Basic constructor that calls the constructor of the class vpMeTracker. + */ vpMeEllipse(); + /*! + * Copy constructor. + */ vpMeEllipse(const vpMeEllipse &me_ellipse); + /*! + * Destructor. + */ virtual ~vpMeEllipse(); - + /*! + * Display the ellipse or arc of ellipse + * + * \warning To effectively display the ellipse a call to + * vpDisplay::flush() is needed. + * + * \param I : Image in which the ellipse appears. + * \param col : Color of the displayed ellipse. + * \param thickness : Thickness of the drawing. + */ void display(const vpImage &I, const vpColor &col, unsigned int thickness = 1); /*! - Gets the second order normalized centered moment \f$ n_{ij} \f$ - as a 3-dim vector containing \f$ n_{20}, n_{11}, n_{02} \f$ - such as \f$ n_{ij} = \mu_{ij}/m_{00} \f$ - - \return The 3-dim vector containing \f$ n_{20}, n_{11}, n_{02} \f$. - - \sa getCenter(), get_ABE(), getArea() - */ + * Gets the second order normalized centered moment \f$ n_{ij} \f$ + * as a 3-dim vector containing \f$ n_{20}, n_{11}, n_{02} \f$ + * such as \f$ n_{ij} = \mu_{ij}/m_{00} \f$ + * + * \return The 3-dim vector containing \f$ n_{20}, n_{11}, n_{02} \f$. + * + * \sa getCenter(), get_ABE(), getArea() + */ inline vpColVector get_nij() const { vpColVector nij(3); @@ -117,14 +133,14 @@ class VISP_EXPORT vpMeEllipse : public vpMeTracker } /*! - Gets the ellipse parameters as a 3-dim vector containing \f$ A, B, E \f$. - - \return The 3-dim vector containing \f$ A, B, E \f$ corresponding respectively to - the semi major axis, the semi minor axis and the angle in rad made by the major axis - and the u axis of the image frame \f$ (u,v) \f$, \f$ e \in [-\pi/2;pi/2] \f$. - - \sa getCenter(), get_nij(), getArea() - */ + * Gets the ellipse parameters as a 3-dim vector containing \f$ A, B, E \f$. + * + * \return The 3-dim vector containing \f$ A, B, E \f$ corresponding respectively to + * the semi major axis, the semi minor axis and the angle in rad made by the major axis + * and the u axis of the image frame \f$ (u,v) \f$, \f$ e \in [-\pi/2;pi/2] \f$. + * + * \sa getCenter(), get_nij(), getArea() + */ inline vpColVector get_ABE() const { vpColVector ABE(3); @@ -136,74 +152,125 @@ class VISP_EXPORT vpMeEllipse : public vpMeTracker } /*! - Gets the area of the ellipse corresponding also to the zero - order moment of the ellipse. - - \return The ellipse area. - - \sa getCenter(), get_nij(), get_ABE() - */ + * Gets the area of the ellipse corresponding also to the zero + * order moment of the ellipse. + * + * \return The ellipse area. + * + * \sa getCenter(), get_nij(), get_ABE() + */ inline double getArea() const { return m00; } /*! - Gets the center of the ellipse. - - \sa get_nij(), get_ABE(), getArea() - */ + * Gets the center of the ellipse. + * + * \sa get_nij(), get_ABE(), getArea() + */ inline vpImagePoint getCenter() const { return iPc; } /*! - \return Expected number of moving edges to track along the ellipse. + * \return Expected number of moving edges to track along the ellipse. */ unsigned int getExpectedDensity() const { return m_expectedDensity; } /*! - \return Number of valid edges tracked along the ellipse. - */ + * \return Number of valid edges tracked along the ellipse. + */ unsigned int getNumberOfGoodPoints() const { return m_numberOfGoodPoints; } /*! - Gets the first endpoint of the ellipse arc (corresponding to alpha1, - not alphamin) when an arc is tracked. - - \sa getSecondEndpoint() - */ + * Gets the first endpoint of the ellipse arc (corresponding to alpha1, + * not alphamin) when an arc is tracked. + * + * \sa getSecondEndpoint() + */ inline vpImagePoint getFirstEndpoint() const { return iP1; } /*! - Gets the highest \f$ alpha \f$ angle of the moving edges tracked - \f$ \alpha_{max} \in [\alpha_min;\alpha_2] \f$. - - \sa getSmallestAngle() - */ + * Gets the highest \f$ alpha \f$ angle of the moving edges tracked + * \f$ \alpha_{max} \in [\alpha_min;\alpha_2] \f$. + * + * \sa getSmallestAngle() + */ inline double getHighestAngle() const { return m_alphamax; } /*! - Gets the second endpoint of the ellipse arc (corresponding to alpha2, - not alphamax) when an arc is tracked. - - \sa getFirstEndpoint() - */ + * Gets the second endpoint of the ellipse arc (corresponding to alpha2, + * not alphamax) when an arc is tracked. + * + * \sa getFirstEndpoint() + */ inline vpImagePoint getSecondEndpoint() const { return iP2; } /*! - Gets the smallest \f$ alpha \f$ angle of the moving edges tracked - \f$ \alpha_{min} \in [\alpha_1;\alpha_2] \f$. - - \sa getHighestAngle() - */ + * Gets the smallest \f$ alpha \f$ angle of the moving edges tracked + * \f$ \alpha_{min} \in [\alpha_1;\alpha_2] \f$. + * + * \sa getHighestAngle() + */ inline double getSmallestAngle() const { return m_alphamin; } + /*! + * Initialize the tracking of an ellipse or an arc of an ellipse when \e trackArc is set to true. + * Ask the user to click on five points located on the ellipse to be tracked. + * + * \warning The points should be selected as far as possible from each other. + * When an arc of an ellipse is tracked, it is recommended to select the 5 points clockwise. + * + * \param I : Image in which the ellipse appears. + * \param trackCircle : When true, track a circle, when false track an ellipse. + * \param trackArc : When true track an arc of the ellipse/circle. In that case, first and + * last points specify the extremities of the arc (clockwise). + * When false track the complete ellipse/circle. + */ void initTracking(const vpImage &I, bool trackCircle = false, bool trackArc = false); + + /*! + * Initialize the tracking of an ellipse/circle or an arc of an ellipse/circle when \e trackArc is set to true. + * The ellipse/circle is defined thanks to a vector of image points. + * + * \warning It is mandatory to use at least five image points to estimate the + * ellipse parameters while three points are needed to estimate the circle parameters. + * \warning The image points should be selected as far as possible from each other. + * When an arc of an ellipse/circle is tracked, it is recommended to select the 5/3 points clockwise. + * + * \param I : Image in which the ellipse/circle appears. + * \param iP : A vector of image points belonging to the ellipse/circle edge used to + * initialize the tracking. + * \param trackCircle : When true, track a circle, when false track an ellipse. + * \param trackArc : When true track an arc of the ellipse/circle. In that case, first and + * last points specify the extremities of the arc (clockwise). + * When false track the complete ellipse/circle. + */ void initTracking(const vpImage &I, const std::vector &iP, bool trackCircle = false, bool trackArc = false); + + /*! + * Initialize the tracking of an ellipse/circle or an arc of an ellipse/circle when arc extremities are given. + * The ellipse/circle is defined by the vector containing the coordinates of its center and the three second order + * centered normalized moments \f$ n_ij \f$. Without setting the arc extremities with + * parameters \e pt1 and \e pt2, the complete ellipse/circle is considered. When extremities + * are set, we consider an ellipse/circle arc defined clockwise from first extremity to second extremity. + * + * \param I : Image in which the ellipse appears. + * \param param : Vector with the five parameters \f$(u_c, v_c, n_{20}, n_{11}, n_{02})\f$ defining the ellipse + * (expressed in pixels). + * \param pt1 : Image point defining the first extremity of the arc or NULL to track a complete ellipse. + * \param pt2 : Image point defining the second extremity of the arc or NULL to track a complete ellipse. + * \param trackCircle : When true enable tracking of a circle, when false the tracking of an ellipse. + */ void initTracking(const vpImage &I, const vpColVector ¶m, vpImagePoint *pt1 = NULL, const vpImagePoint *pt2 = NULL, bool trackCircle = false); + + /*! + * Print the parameters \f$ K = {K_0, ..., K_5} \f$, the coordinates of the + * ellipse center, the normalized moments, and the A, B, E parameters. + */ void printParameters() const; /*! - Set the two endpoints of the ellipse arc when an arc is tracked. - */ + * Set the two endpoints of the ellipse arc when an arc is tracked. + */ void setEndpoints(const vpImagePoint &pt1, const vpImagePoint &pt2) { iP1 = pt1; @@ -211,14 +278,14 @@ class VISP_EXPORT vpMeEllipse : public vpMeTracker } /*! - Set the threshold for the weights in the robust estimation of the - ellipse parameters. - If the weight of a point is below this threshold, this one is removed from - the list of tracked meSite. - Value must be between 0 (never rejected) and 1 (always rejected). - - \param threshold : The new value of the threshold. - */ + * Set the threshold for the weights in the robust estimation of the + * ellipse parameters. + * If the weight of a point is below this threshold, this one is removed from + * the list of tracked meSite. + * Value must be between 0 (never rejected) and 1 (always rejected). + * + * \param threshold : The new value of the threshold. + */ void setThresholdRobust(double threshold) { if (threshold < 0) { @@ -233,9 +300,9 @@ class VISP_EXPORT vpMeEllipse : public vpMeTracker } /*! - Track a set of MEs along an ellipse or a circle. - The number of valid tracked MEs is obtained from getNumberOfGoodPoints(). - */ + * Track a set of MEs along an ellipse or a circle. + * The number of valid tracked MEs is obtained from getNumberOfGoodPoints(). + */ void track(const vpImage &I); #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS @@ -427,8 +494,12 @@ class VISP_EXPORT vpMeEllipse : public vpMeTracker double m_uc; //! Value of v coordinate of iPc double m_vc; - //! Second order centered and normalized moments - double m_n20, m_n11, m_n02; + //! Second order centered and normalized moments \f$ n_{20} \f$ + double m_n20; + //! Second order centered and normalized moments \f$ n_{11} \f$ + double m_n11; + //! Second order centered and normalized moments \f$ n_{02} \f$ + double m_n02; //! Expected number of points to track along the ellipse. unsigned int m_expectedDensity; //! Number of correct points tracked along the ellipse. @@ -441,23 +512,128 @@ class VISP_EXPORT vpMeEllipse : public vpMeTracker double m_arcEpsilon; protected: + /*! + * Computes the length of the semi major axis \f$ a \f$, the length of the + * semi minor axis \f$ b \f$, and \f$ e \f$ that is the angle + * made by the major axis and the u axis of the image frame \f$ (u,v) \f$. + * They are computed from the normalized moments $ \f$ n_{ij} \f$. + */ void computeAbeFromNij(); + + /*! + * Compute the angle of a point on the ellipse wrt the ellipse major axis. + * \param pt : Image point on the ellipse. + * \return The computed angle. + */ double computeAngleOnEllipse(const vpImagePoint &pt) const; + + /*! + * Computes the parameters \f$ K = {K_0, ..., K_5} \f$ from the center of + * the ellipse and the normalized moments \f$ n_{ij} \f$. The parameters + * \f$ K \f$ are such that \f$ K0 = n02, K1 = n20 \f$, etc. as in Eq (25) + * of Chaumette 2004 TRO paper. + */ void computeKiFromNij(); + + /*! + * Computes the normalized moments \f$ n_{ij} \f$ from the \f$ A, B, E \f$ + * parameters as in Eq (24) of Chaumette 2004 TRO paper after simplifications + * to deal with the case cos(e) = 0.0 + */ void computeNijFromAbe(); - void computePointOnEllipse(const double ang, vpImagePoint &iP); + + /*! + * Compute the coordinates of a point on an ellipse from its angle with respect + * to the main orientation of the ellipse. + * + * \param angle : Angle on the ellipse with respect to its major axis. + * \param iP : Image point on the ellipse. + */ + void computePointOnEllipse(const double angle, vpImagePoint &iP); + + /*! + * Computes the \f$ \theta \f$ angle that represents the angle between the + * tangent to the curve and the u axis. This angle is used for tracking the + * vpMeSite. + * + * \param iP : The point belonging to the ellipse where the angle is computed. + */ double computeTheta(const vpImagePoint &iP) const; + + /*! + * Computes the \f$ \theta \f$ angle that represents the angle between the + * tangent to the curve and the u axis. This angle is used for tracking the + * vpMeSite. + * + * \param u,v : The point belonging to the ellipse where the angle is computed. + */ double computeTheta(double u, double v) const; + + /*! + * Computes the coordinates of the ellipse center, the normalized + * moments \f$ n_{ij} \f$, the length of the semi major axis \f$ a \f$, the + * length of the semi minor axis \f$ b \f$, and \f$ e \f$ that is the angle + * made by the major axis and the u axis of the image frame \f$ (u,v) \f$. + * + * All those computations are made from the parameters \f$ K ={K_0, ..., K_5} \f$ + * so that \f$ K_0 u^2 + K1 v^2 + 2 K2 u v + 2 K3 u + 2 K4 v + K5 = 0 \f$. + */ void getParameters(); + + /*! + * Least squares method to compute the circle/ ellipse to which the points belong. + * + * \param I : Image in which the circle/ellipse appears (useful just to get + * its number of rows and columns... + * \param iP : A vector of points belonging to the circle/ellipse. + */ void leastSquare(const vpImage &I, const std::vector &iP); + + /*! + * Robust least squares method to compute the ellipse to which the vpMeSite + * belong. Manage also the lists of vpMeSite and corresponding angles, + * and update the expected density of points. + * + * \param I : Image where tracking is done (useful just to get its number + * of rows and columns... + */ unsigned int leastSquareRobust(const vpImage &I); + + /*! + * 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. + * + * \param I : Image in which the ellipse appears. + * + * \return The function returns the number of points added to the list. + * + * \exception vpTrackingException::initializationError : Moving edges not + * initialized. + */ unsigned int plugHoles(const vpImage &I); + /*! + * Construct a list of vpMeSite moving edges at a particular sampling + * step between the two extremities. The two extremities are defined by + * the points with the smallest and the biggest \f$ alpha \f$ angle. + * + * \param I : Image in which the ellipse appears. + * \param doNotTrack : If true, moving-edges are not tracked. + * + * \exception vpTrackingException::initializationError : Moving edges not + * initialized. + */ #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) virtual void sample(const vpImage &I, bool doNotTrack = false) override; #else virtual void sample(const vpImage &I, bool doNotTrack = false); #endif + + /*! + * Compute the \f$ theta \f$ angle for each vpMeSite. + * + * \note The \f$ theta \f$ angle is useful during the tracking part. + */ void updateTheta(); private: @@ -467,21 +643,54 @@ class VISP_EXPORT vpMeEllipse : public vpMeTracker // Static Function public: + /*! + * Display the ellipse or the arc of ellipse thanks to the ellipse parameters. + * + * \param I : The image used as background. + * \param center : Center of the ellipse. + * \param A : Semi major axis of the ellipse. + * \param B : Semi minor axis of the ellipse. + * \param E : Angle made by the major axis and the u axis of the image frame + * \f$ (u,v) \f$ (in rad). + * \param smallalpha : Smallest \f$ alpha \f$ angle in rad (0 for a complete ellipse). + * \param highalpha : Highest \f$ alpha \f$ angle in rad (2 \f$ \Pi \f$ for a complete ellipse). + * \param color : Color used to display the ellipse. + * \param thickness : Thickness of the drawings. + * + * \sa vpDisplay::displayEllipse() + */ static void displayEllipse(const vpImage &I, const vpImagePoint ¢er, const double &A, const double &B, - const double &E, const double &smallalpha, const double &highalpha, - const vpColor &color = vpColor::green, unsigned int thickness = 1); + const double &E, const double &smallalpha, const double &highalpha, + const vpColor &color = vpColor::green, unsigned int thickness = 1); + + /*! + * Display the ellipse or the arc of ellipse thanks to the ellipse parameters. + * + * \param I : The image used as background. + * \param center : Center of the ellipse + * \param A : Semimajor axis of the ellipse. + * \param B : Semiminor axis of the ellipse. + * \param E : Angle made by the major axis and the u axis of the image frame + * \f$ (u,v) \f$ (in rad) + * \param smallalpha : Smallest \f$ alpha \f$ angle in rad (0 for a complete ellipse) + * \param highalpha : Highest \f$ alpha \f$ angle in rad (\f$ 2 \Pi \f$ for a complete ellipse) + * \param color : Color used to display th lines. + * \param thickness : Thickness of the drawings. + * + * \sa vpDisplay::displayEllipse() + */ static void displayEllipse(const vpImage &I, const vpImagePoint ¢er, const double &A, const double &B, - const double &E, const double &smallalpha, const double &highalpha, - const vpColor &color = vpColor::green, unsigned int thickness = 1); + const double &E, const double &smallalpha, const double &highalpha, + const vpColor &color = vpColor::green, unsigned int thickness = 1); #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS // Marked deprecated since they override vpMeTracker::display(). Warning detected by mingw64 vp_deprecated static void display(const vpImage &I, const vpImagePoint ¢er, const double &A, const double &B, - const double &E, const double &smallalpha, const double &highalpha, - const vpColor &color = vpColor::green, unsigned int thickness = 1); + const double &E, const double &smallalpha, const double &highalpha, + const vpColor &color = vpColor::green, unsigned int thickness = 1); vp_deprecated static void display(const vpImage &I, const vpImagePoint ¢er, const double &A, const double &B, - const double &E, const double &smallalpha, const double &highalpha, - const vpColor &color = vpColor::green, unsigned int thickness = 1); + const double &E, const double &smallalpha, const double &highalpha, + const vpColor &color = vpColor::green, unsigned int thickness = 1); #endif }; diff --git a/modules/tracker/me/include/visp3/me/vpMeLine.h b/modules/tracker/me/include/visp3/me/vpMeLine.h index 8cca0f2691..ebc8a9ec15 100644 --- a/modules/tracker/me/include/visp3/me/vpMeLine.h +++ b/modules/tracker/me/include/visp3/me/vpMeLine.h @@ -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: * Moving edges. - * -*****************************************************************************/ + */ /*! \file vpMeLine.h @@ -152,12 +150,15 @@ class VISP_EXPORT vpMeLine : public vpMeTracker static void update_indices(double theta, int incr, int i, int j, int &i1, int &i2, int &j1, int &j2); protected: - vpMeSite PExt[2]; + vpMeSite PExt[2]; //!< Line extremities - double rho, theta; - double delta, delta_1; - double angle, angle_1; - int sign; + double rho; //!< rho parameter of the line + double theta; //!< theta parameter of the line + double delta; //!< Angle in rad between the extremities + double delta_1; //!< Angle in rad between the extremities + double angle; //!< Angle in deg between the extremities + double angle_1; //!< Angle in deg between the extremities + int sign; //!< Sign //! Flag to specify wether the intensity of the image at the middle point is //! used to compute the sign of rho or not. @@ -174,31 +175,149 @@ class VISP_EXPORT vpMeLine : public vpMeTracker double c; //!< Parameter c of the line equation a*i + b*j + c = 0 public: + /*! + * Basic constructor that calls the constructor of the class vpMeTracker. + */ vpMeLine(); + + /*! + * Copy constructor. + */ vpMeLine(const vpMeLine &meline); + + /*! + * Destructor. + */ virtual ~vpMeLine(); + /*! + * Display line. + * + * \warning To effectively display the line a call to + * vpDisplay::flush() is needed. + * + * \param I : Image in which the line appears. + * \param color : Color of the displayed line. Note that a moving edge + * that is considered as an outlier is displayed in green. + * \param thickness : Drawings thickness. + */ void display(const vpImage &I, const vpColor &color, unsigned int thickness = 1); - void track(const vpImage &Im); + /*! + * Track the line in the image I. + * + * \param I : Image in which the line appears. + */ + void track(const vpImage &I); + + /*! + * Construct a list of vpMeSite moving edges at a particular sampling + * step between the two extremities of the line. + * + * \param I : Image in which the line appears. + * \param doNotTrack : Inherited parameter, not used. + * + * \exception vpTrackingException::initializationError : Moving edges not + * initialized. + */ #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) - virtual void sample(const vpImage &image, bool doNotTrack = false) override; + virtual void sample(const vpImage &I, bool doNotTrack = false) override; #else - virtual void sample(const vpImage &image, bool doNotTrack = false); + virtual void sample(const vpImage &I, bool doNotTrack = false); #endif + + /*! + * Resample the line if the number of sample is less than 80% of the + * expected value. + * + * \note The expected value is computed thanks to the length of the + * line and the parameter which indicates the number of pixel between + * two points (vpMe::sample_step). + * + * \param I : Image in which the line appears. + */ void reSample(const vpImage &I); + + /*! + * Least squares method used to make the tracking more robust. It + * ensures that the points taken into account to compute the right + * equation belong to the line. + */ void leastSquare(); + + /*! + * Set the alpha value of the different vpMeSite to the value of delta. + */ void updateDelta(); + + /*! + * Seek in the list of available points the two extremities of the line. + */ void setExtremities(); + + /*! + * Seek along the line defined by its equation, the two extremities of + * the line. This function is useful in case of translation of the + * line. + * + * \param I : Image in which the line appears. + * + * \exception vpTrackingException::initializationError : Moving edges not + * initialized. + */ void seekExtremities(const vpImage &I); + + /*! + * Suppression of the points which belong no more to the line. + */ void suppressPoints(); + /*! + * Initialization of the tracking. Ask the user to click on two points + * from the line to track. + * + * \param I : Image in which the line appears. + */ void initTracking(const vpImage &I); + + /*! + * Initialization of the tracking. The line is defined thanks to the + * coordinates of two points. + * + * \param I : Image in which the line appears. + * \param ip1 : Coordinates of the first point. + * \param ip2 : Coordinates of the second point. + */ void initTracking(const vpImage &I, const vpImagePoint &ip1, const vpImagePoint &ip2); + /*! + * Compute the two parameters \f$(\rho, \theta)\f$ of the line. + * + * \param I : Image in which the line appears. + */ void computeRhoTheta(const vpImage &I); + + /*! + * Get the value of \f$\rho\f$, the distance between the origin and the + * point on the line with belong to the normal to the line crossing + * the origin. + * + * Depending on the convention described at the beginning of this + * class, \f$\rho\f$ is signed. + */ double getRho() const; + + /*! + * Get the value of the angle \f$\theta\f$. + */ double getTheta() const; + + /*! + * Get the extremities of the line. + * + * \param ip1 : Coordinates of the first extremity. + * \param ip2 : Coordinates of the second extremity. + */ void getExtremities(vpImagePoint &ip1, vpImagePoint &ip2); /*! @@ -226,6 +345,17 @@ class VISP_EXPORT vpMeLine : public vpMeTracker */ inline double getC() const { return c; } + /*! + * Computes the intersection point of two lines. The result is given in + * the (i,j) frame. + * + * \param line1 : The first line. + * \param line2 : The second line. + * \param ip : The coordinates of the intersection point. + * + * \return Returns a boolean value which depends on the computation + * success. True means that the computation ends successfully. + */ static bool intersection(const vpMeLine &line1, const vpMeLine &line2, vpImagePoint &ip); /*! @@ -241,34 +371,90 @@ class VISP_EXPORT vpMeLine : public vpMeTracker // Static Functions public: + /*! + * Display of a moving line thanks to its equation parameters and its + * extremities. + * + * \param I : The image used as background. + * \param PExt1 : First extremity + * \param PExt2 : Second extremity + * \param A : Parameter a of the line equation a*i + b*j + c = 0 + * \param B : Parameter b of the line equation a*i + b*j + c = 0 + * \param C : Parameter c of the line equation a*i + b*j + c = 0 + * \param color : Color used to display the line. + * \param thickness : Thickness of the line. + */ static void displayLine(const vpImage &I, const vpMeSite &PExt1, const vpMeSite &PExt2, const double &A, - const double &B, const double &C, const vpColor &color = vpColor::green, - unsigned int thickness = 1); + const double &B, const double &C, const vpColor &color = vpColor::green, + unsigned int thickness = 1); + + /*! + * Display of a moving line thanks to its equation parameters and its + * extremities. + * + * \param I : The image used as background. + * \param PExt1 : First extremity + * \param PExt2 : Second extremity + * \param A : Parameter a of the line equation a*i + b*j + c = 0 + * \param B : Parameter b of the line equation a*i + b*j + c = 0 + * \param C : Parameter c of the line equation a*i + b*j + c = 0 + * \param color : Color used to display the line. + * \param thickness : Thickness of the line. + */ static void displayLine(const vpImage &I, const vpMeSite &PExt1, const vpMeSite &PExt2, const double &A, - const double &B, const double &C, const vpColor &color = vpColor::green, - unsigned int thickness = 1); + const double &B, const double &C, const vpColor &color = vpColor::green, + unsigned int thickness = 1); + /*! + * Display of a moving line thanks to its equation parameters and its + * extremities with all the site list. + * + * \param I : The image used as background. + * \param PExt1 : First extremity + * \param PExt2 : Second extremity + * \param site_list : vpMeSite list + * \param A : Parameter a of the line equation a*i + b*j + c = 0 + * \param B : Parameter b of the line equation a*i + b*j + c = 0 + * \param C : Parameter c of the line equation a*i + b*j + c = 0 + * \param color : Color used to display the line. + * \param thickness : Thickness of the line. + */ static void displayLine(const vpImage &I, const vpMeSite &PExt1, const vpMeSite &PExt2, - const std::list &site_list, const double &A, const double &B, const double &C, - const vpColor &color = vpColor::green, unsigned int thickness = 1); + const std::list &site_list, const double &A, const double &B, const double &C, + const vpColor &color = vpColor::green, unsigned int thickness = 1); + + /*! + * Display of a moving line thanks to its equation parameters and its + * extremities with all the site list. + * + * \param I : The image used as background. + * \param PExt1 : First extremity + * \param PExt2 : Second extremity + * \param site_list : vpMeSite list + * \param A : Parameter a of the line equation a*i + b*j + c = 0 + * \param B : Parameter b of the line equation a*i + b*j + c = 0 + * \param C : Parameter c of the line equation a*i + b*j + c = 0 + * \param color : Color used to display the line. + * \param thickness : Thickness of the line. + */ static void displayLine(const vpImage &I, const vpMeSite &PExt1, const vpMeSite &PExt2, - const std::list &site_list, const double &A, const double &B, const double &C, - const vpColor &color = vpColor::green, unsigned int thickness = 1); + const std::list &site_list, const double &A, const double &B, const double &C, + const vpColor &color = vpColor::green, unsigned int thickness = 1); #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS vp_deprecated static void display(const vpImage &I, const vpMeSite &PExt1, const vpMeSite &PExt2, const double &A, - const double &B, const double &C, const vpColor &color = vpColor::green, - unsigned int thickness = 1); + const double &B, const double &C, const vpColor &color = vpColor::green, + unsigned int thickness = 1); vp_deprecated static void display(const vpImage &I, const vpMeSite &PExt1, const vpMeSite &PExt2, const double &A, - const double &B, const double &C, const vpColor &color = vpColor::green, - unsigned int thickness = 1); + const double &B, const double &C, const vpColor &color = vpColor::green, + unsigned int thickness = 1); vp_deprecated static void display(const vpImage &I, const vpMeSite &PExt1, const vpMeSite &PExt2, - const std::list &site_list, const double &A, const double &B, const double &C, - const vpColor &color = vpColor::green, unsigned int thickness = 1); + const std::list &site_list, const double &A, const double &B, const double &C, + const vpColor &color = vpColor::green, unsigned int thickness = 1); vp_deprecated static void display(const vpImage &I, const vpMeSite &PExt1, const vpMeSite &PExt2, - const std::list &site_list, const double &A, const double &B, const double &C, - const vpColor &color = vpColor::green, unsigned int thickness = 1); + const std::list &site_list, const double &A, const double &B, const double &C, + const vpColor &color = vpColor::green, unsigned int thickness = 1); #endif }; diff --git a/modules/tracker/me/include/visp3/me/vpMeNurbs.h b/modules/tracker/me/include/visp3/me/vpMeNurbs.h index 01bd7dccca..74e70c848c 100644 --- a/modules/tracker/me/include/visp3/me/vpMeNurbs.h +++ b/modules/tracker/me/include/visp3/me/vpMeNurbs.h @@ -1,4 +1,4 @@ -/**************************************************************************** +/* * * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. @@ -30,8 +30,7 @@ * * Description: * Moving edges. - * -*****************************************************************************/ + */ /*! \file vpMeNurbs.h @@ -59,11 +58,11 @@ \brief Class that tracks in an image a edge defined by a Nurbs. The advantage of this class is that it enables to track an edge whose -equation is not known in advance. At each iteration, the Nurbs corresponding -to the edge is computed. + equation is not known in advance. At each iteration, the Nurbs corresponding + to the edge is computed. It is possible to have a direct access to the nurbs. It is indeed a public -parameter. + parameter. The code below shows how to use this class. \code @@ -156,51 +155,150 @@ class VISP_EXPORT vpMeNurbs : public vpMeTracker float cannyTh2; public: + /*! + * Basic constructor that calls the constructor of the class vpMeTracker. + */ vpMeNurbs(); + + /*! + * Copy constructor. + */ vpMeNurbs(const vpMeNurbs &menurbs); - virtual ~vpMeNurbs(); /*! - Sets the number of control points used to compute the Nurbs. + * Destructor. + */ + virtual ~vpMeNurbs(); - \param nb_point : The number of control points used to compute the Nurbs. - */ + /*! + * Sets the number of control points used to compute the Nurbs. + * + * \param nb_point : The number of control points used to compute the Nurbs. + */ void setNbControlPoints(unsigned int nb_point) { this->nbControlPoints = nb_point; } /*! - Enables or disables the canny detection used during the extremities - search. - - \param enable_canny : if true it enables the canny detection. - */ + * Enables or disables the canny detection used during the extremities + * search. + * + * \param enable_canny : if true it enables the canny detection. + */ void setEnableCannyDetection(const bool enable_canny) { this->enableCannyDetection = enable_canny; } /*! - Enables to set the two thresholds use by the canny detection. - - \param th1 : The first threshold; - \param th2 : The second threshold; - */ + * Enables to set the two thresholds use by the canny detection. + * + * \param th1 : The first threshold; + * \param th2 : The second threshold; + */ void setCannyThreshold(float th1, float th2) { this->cannyTh1 = th1; this->cannyTh2 = th2; } + /*! + * Initialization of the tracking. Ask the user to click left on several points + * along the edge to track and click right at the end. + * + * \param I : Image in which the edge appears. + */ void initTracking(const vpImage &I); + + /*! + * Initialization of the tracking. The Nurbs is initialized thanks to the + * list of vpImagePoint. + * + * \param I : Image in which the edge appears. + * \param ptList : List of point to initialize the Nurbs. + */ void initTracking(const vpImage &I, const std::list &ptList); + /*! + * Track the edge in the image I. + * + * \param I : Image in which the edge appears. + */ void track(const vpImage &I); - virtual void sample(const vpImage &image, bool doNotTrack = false); + /*! + * Construct a list of vpMeSite moving edges at a particular sampling + * step between the two extremities of the nurbs. + * + * \param I : Image in which the edge appears. + * \param doNotTrack : Inherited parameter, not used. + */ + virtual void sample(const vpImage &I, bool doNotTrack = false); + + /*! + * Resample the edge if the number of sample is less than 70% of the + * expected value. + * + * \note The expected value is computed thanks to the length of the + * nurbs and the parameter which indicates the number of pixel between + * two points (vpMe::sample_step). + * + * \param I : Image in which the edge appears. + */ void reSample(const vpImage &I); + + /*! + * Set the alpha value (normal to the edge at this point) + * of the different vpMeSite to a value computed thanks to the nurbs. + */ void updateDelta(); + + /*! + * Seek along the edge defined by the nurbs, the two extremities of + * the edge. This function is useful in case of translation of the + * edge. + */ void setExtremities(); + + /*! + * Seek along the edge defined by the nurbs, the two extremities of + * the edge. This function is useful in case of translation of the + * edge. + * + * \param I : Image in which the edge appears. + */ void seekExtremities(const vpImage &I); + + /*! + * Seek the extremities of the edge thanks to a canny edge detection. + * The edge detection enable to find the points belonging to the edge. + * The any vpMeSite are initialized at this points. + * + * This method is useful when the edge is not smooth. + * + * \note To use the canny detection, OpenCV has to be installed. + * + * \param I : Image in which the edge appears. + */ void seekExtremitiesCanny(const vpImage &I); + + /*! + * Suppression of the points which: + * + * - belong no more to the edge. + * - which are to closed to another point. + */ void suppressPoints(); + /*! + * Suppress vpMeSites if they are too close to each other. + * + * The goal is to keep the order of the vpMeSites in the list. + */ void supressNearPoints(); + + /*! + * Resample a part of the edge if two vpMeSite are too far from each other. + * In this case the method try to initialize any vpMeSite between the two + * points. + * + * \param I : Image in which the edge appears. + */ void localReSample(const vpImage &I); /*! @@ -208,6 +306,16 @@ class VISP_EXPORT vpMeNurbs : public vpMeTracker */ inline vpNurbs getNurbs() const { return nurbs; } + /*! + * Display edge. + * + * \warning To effectively display the edge a call to + * vpDisplay::flush() is needed. + * + * \param I : Image in which the edge appears. + * \param color : Color of the displayed line. + * \param thickness : Drawings thickness. + */ void display(const vpImage &I, const vpColor &color, unsigned int thickness = 1); private: @@ -222,7 +330,24 @@ class VISP_EXPORT vpMeNurbs : public vpMeTracker bool farFromImageEdge(const vpImage &I, const vpImagePoint &iP); public: + /*! + * Display of a moving nurbs. + * + * \param I : The image used as background. + * \param n : Nurbs to display + * \param color : Color used to display the nurbs. + * \param thickness : Drawings thickness. + */ static void display(const vpImage &I, vpNurbs &n, const vpColor &color = vpColor::green, unsigned int thickness = 1); + + /*! + * Display of a moving nurbs. + * + * \param I : The image used as background. + * \param n : Nurbs to display + * \param color : Color used to display the nurbs. + * \param thickness : Drawings thickness. + */ static void display(const vpImage &I, vpNurbs &n, const vpColor &color = vpColor::green, unsigned int thickness = 1); }; diff --git a/modules/tracker/me/include/visp3/me/vpMeSite.h b/modules/tracker/me/include/visp3/me/vpMeSite.h index 60218f739a..9865de8c7b 100644 --- a/modules/tracker/me/include/visp3/me/vpMeSite.h +++ b/modules/tracker/me/include/visp3/me/vpMeSite.h @@ -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: * Moving edges. - * -*****************************************************************************/ + */ /*! \file vpMeSite.h @@ -66,9 +64,20 @@ class VISP_EXPORT vpMeSite { public: - typedef enum { NONE, RANGE, RESULT, RANGE_RESULT } vpMeSiteDisplayType; + /*! + * Type moving-edges site of display. + */ + typedef enum + { + NONE, //!< Not displayed + RANGE, //!< + RESULT, //!< + RANGE_RESULT //!< + } vpMeSiteDisplayType; - /// Moving-edge site state + /*! + * Moving-edge site state + */ typedef enum { NO_SUPPRESSION = 0, ///< Point used by the tracker. @@ -83,17 +92,23 @@ class VISP_EXPORT vpMeSite } vpMeSiteState; public: - int i, j; - double ifloat, jfloat; - unsigned char v; + //! Coordinate along i of a site + int i; + //! Coordinates along j of a site + int j; + //! Floating coordinates along i of a site + double ifloat; + //! Floating coordinates along j of a site + double jfloat; + //! Mask sign int mask_sign; - // Angle of tangent at site + //! Angle of tangent at site double alpha; - // Convolution of Site in previous image + //! Convolution of Site in previous image double convlt; - // Convolution of Site in previous image + //! Convolution of Site in previous image double normGradient; - // Uncertainty of point given as a probability between 0 and 1 + //! Uncertainty of point given as a probability between 0 and 1 double weight; private: @@ -101,76 +116,134 @@ class VISP_EXPORT vpMeSite vpMeSiteState state; public: + /*! + * Initialize moving-edge site with default parameters. + */ void init(); + + /*! + * Initialize moving-edge site parameters. + */ void init(double ip, double jp, double alphap); + + /*! + * Initialize moving-edge site parameters. + */ void init(double ip, double jp, double alphap, double convltp); + + /*! + * Initialize moving-edge site parameters. + */ void init(double ip, double jp, double alphap, double convltp, int sign); + /*! + * Default constructor. + */ vpMeSite(); + + /*! + * Constructor from pixel coordinates. + */ vpMeSite(double ip, double jp); + + /*! + * Copy constructor. + */ vpMeSite(const vpMeSite &mesite); + + /*! + * Destructor. + */ virtual ~vpMeSite() { }; + /*! + * Display moving edges in image I. + * @param I : Input image. + */ void display(const vpImage &I); + + /*! + * Display moving edges in image I. + * @param I : Input image. + */ void display(const vpImage &I); + /*! + * Compute convolution. + */ double convolution(const vpImage &ima, const vpMe *me); + /*! + * Construct and return the list of vpMeSite along the normal to the contour, + * in the given range. + * \pre : ifloat, jfloat, and the direction of the normal (alpha) have to be set. + * \param I : Image in which the display is performed. + * \param range : +/- the range within which the pixel's correspondent will be sought. + * \return Pointer to the list of query sites + */ vpMeSite *getQueryList(const vpImage &I, const int range); + /*! + * Specific function for moving-edges. + * + * \warning To display the moving edges graphics a call to vpDisplay::flush() is needed after this function. + */ void track(const vpImage &im, const vpMe *me, bool test_likelihood = true); /*! - Set the angle of tangent at site - - \param a : new value of alpha - */ + * Set the angle of tangent at site. + * + * \param a : new value of alpha + */ void setAlpha(const double &a) { alpha = a; } /*! - Get the angle of tangent at site - - \return value of alpha - */ + * Get the angle of tangent at site. + * + * \return value of alpha + */ inline double getAlpha() const { return alpha; } + /*! + * Display selector. + */ void setDisplay(vpMeSiteDisplayType select) { selectDisplay = select; } /*! - Get the i coordinate (integer) - - \return value of i - */ + * Get the i coordinate (integer). + * + * \return value of i + */ inline int get_i() const { return i; } /*! - Get the j coordinate (f) - - \return value of j - */ + * Get the j coordinate (f). + * + * \return value of j + */ inline int get_j() const { return j; } /*! - Get the i coordinate (double) - - \return value of i - */ + * Get the i coordinate (double). + * + * \return value of i + */ inline double get_ifloat() const { return ifloat; } /*! - Get the j coordinate (double) - - \return value of j - */ + * Get the j coordinate (double). + * + * \return value of j + */ inline double get_jfloat() const { return jfloat; } /*! - Set the state of the site - - \param flag : flag corresponding to vpMeSiteState - - \sa vpMeSiteState - */ + * Set the state of the site. + * + * \param flag : flag corresponding to vpMeSiteState + * + * \sa vpMeSiteState + */ void setState(const vpMeSiteState &flag) { state = flag; @@ -181,30 +254,39 @@ class VISP_EXPORT vpMeSite } /*! - Get the state of the site - - \return flag corresponding to vpMeSiteState - */ + * Get the state of the site. + * + * \return flag corresponding to vpMeSiteState + */ inline vpMeSiteState getState() const { return state; } /*! - Set the weight of the site - - \param w : new value of weight - */ + * Set the weight of the site. + * + * \param w : new value of weight + */ void setWeight(const double &w) { weight = w; } /*! - Get the weight of the site - - \return value of weight - */ + * Get the weight of the site. + * + * \return value of weight + */ inline double getWeight() const { return weight; } - // Operators + /*! + * Copy operator. + */ vpMeSite &operator=(const vpMeSite &m); + + /*! + * Comparison operator. + */ int operator!=(const vpMeSite &m); + /*! + * ostream operator. + */ friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, vpMeSite &vpMeS); // Static functions @@ -240,10 +322,41 @@ class VISP_EXPORT vpMeSite return (vpMath::sqr(S1.ifloat - S2.ifloat) + vpMath::sqr(S1.jfloat - S2.jfloat)); } + /*! + * Display the moving edge site with a color corresponding to their state. + * + * - If green : The vpMeSite is a good point. + * - If blue : The point is removed because of the vpMeSite tracking phase (contrast problem). + * - If purple : The point is removed because of the vpMeSite tracking phase (threshold problem). + * - If red : The point is removed because of the robust method in the virtual visual servoing (M-Estimator problem). + * - If cyan : The point is removed because it's too close to another. + * - Yellow otherwise. + * + * \param I : The image. + * \param i : Pixel i of the site. + * \param j : Pixel j of the site. + * \param state : State of the site. + */ static void display(const vpImage &I, const double &i, const double &j, - const vpMeSiteState &state = NO_SUPPRESSION); + const vpMeSiteState &state = NO_SUPPRESSION); + + /*! + * Display the moving edge site with a color corresponding to their state. + * + * - If green : The vpMeSite is a good point. + * - If blue : The point is removed because of the vpMeSite tracking phase (contrast problem). + * - If purple : The point is removed because of the vpMeSite tracking phase (threshold problem). + * - If red : The point is removed because of the robust method in the virtual visual servoing (M-Estimator problem). + * - If cyan : The point is removed because it's too close to another. + * - Yellow otherwise + * + * \param I : The image. + * \param i : Pixel i of the site. + * \param j : Pixel j of the site. + * \param state : State of the site. + */ static void display(const vpImage &I, const double &i, const double &j, - const vpMeSiteState &state = NO_SUPPRESSION); + const vpMeSiteState &state = NO_SUPPRESSION); // Deprecated #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS diff --git a/modules/tracker/me/include/visp3/me/vpMeTracker.h b/modules/tracker/me/include/visp3/me/vpMeTracker.h index 428424bfd4..cdf2f366b5 100644 --- a/modules/tracker/me/include/visp3/me/vpMeTracker.h +++ b/modules/tracker/me/include/visp3/me/vpMeTracker.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2019 by Inria. All rights reserved. * @@ -30,8 +29,7 @@ * * Description: * Moving edges. - * -*****************************************************************************/ + */ /*! \file vpMeTracker.h @@ -74,7 +72,9 @@ class VISP_EXPORT vpMeTracker : public vpTracker std::list list; //! Moving edges initialisation parameters vpMe *me; + //! Initial range unsigned int init_range; + //! Number of good moving-edges that are tracked int nGoodElement; //! Mask used to disable tracking on a part of image const vpImage *m_mask; @@ -87,96 +87,196 @@ class VISP_EXPORT vpMeTracker : public vpTracker //@} public: - // Constructor/Destructor + /*! + * Default constructor. + */ vpMeTracker(); + + /*! + * Copy constructor. + */ vpMeTracker(const vpMeTracker &meTracker); + + /*! + * Destructor. + */ virtual ~vpMeTracker(); /** @name Public Member Functions Inherited from vpMeTracker */ //@{ + + /*! + * Display the moving edge sites with a color corresponding to their state. + * + * - If green : The vpMeSite is a good point. + * - If blue : The point is removed because of the vpMeSite tracking phase (contrast problem). + * - If purple : The point is removed because of the vpMeSite tracking phase (threshold problem). + * - If red : The point is removed because of the robust method in the virtual visual servoing (M-Estimator problem). + * - If cyan : The point is removed because it's too close to another. + * - Yellow otherwise. + * + * \param I : The image. + */ void display(const vpImage &I); + + /*! + * Display the moving edge sites with a color corresponding to their state. + * + * - If green : The vpMeSite is a good point. + * - If blue : The point is removed because of the vpMeSite tracking phase (contrast problem). + * - If purple : The point is removed because of the vpMeSite tracking phase (threshold problem). + * - If red : The point is removed because of the robust method in the virtual visual servoing (M-Estimator problem). + * - If cyan : The point is removed because it's too close to another. + * - Yellow otherwise. + * + * \param I : The image. + */ void display(const vpImage &I); + + /*! + * Displays the status of moving edge sites + * + * \param I : The image. + * \param w : vector + * \param index_w : index + */ void display(const vpImage &I, vpColVector &w, unsigned int &index_w); + /*! + * Test whether the pixel is inside the mask. Mask values that are set to true + * are considered in the tracking. + * + * \param mask: Mask image or NULL if not wanted. Mask values that are set to true + * are considered in the tracking. To disable a pixel, set false. + * \param i : Pixel coordinate along the rows. + * \param j : Pixel coordinate along the columns. + */ static bool inMask(const vpImage *mask, unsigned int i, unsigned int j); /*! - Return the initial range. - - \return Value of init_range. - */ + * Return the initial range. + * + * \return Value of init_range. + */ inline unsigned int getInitRange() { return init_range; } /*! - Return the moving edges initialisation parameters - - \return Moving Edges. - */ + * Return the moving edges initialisation parameters. + * + * \return Moving Edges. + */ inline vpMe *getMe() { return me; } /*! - Return the list of moving edges - - \return List of Moving Edges. - */ + * Return the list of moving edges + * + * \return List of Moving Edges. + */ inline std::list &getMeList() { return list; } - inline std::list getMeList() const { return list; } /*! - Return the number of points that has not been suppressed. + * Return the list of moving edges + * + * \return List of Moving Edges. + */ + inline std::list getMeList() const { return list; } - \return Number of good points. - */ + /*! + * Return the number of points that has not been suppressed. + * + * \return Number of good points. + */ inline int getNbPoints() const { return nGoodElement; } + /*! + * Initialize the tracker. + */ void init(); + + /*! + * Virtual function that is called by lower classes vpMeEllipse, vpMeLine + * and vpMeNurbs. + * + * \exception vpTrackingException::initializationError : Moving edges not + * initialized. + */ void initTracking(const vpImage &I); + /*! + * Return number of moving-edges that are tracked. + */ unsigned int numberOfSignal(); + /*! + * Copy operator. + */ vpMeTracker &operator=(vpMeTracker &f); + /*! + * Check if a pixel i,j is out of the image. + */ int outOfImage(int i, int j, int half, int row, int cols); + /*! + * Check if a pixel i,j is out of the image. + */ int outOfImage(const vpImagePoint &iP, int half, int rows, int cols); + /*! + * Reset the tracker by removing all the moving edges. + */ void reset(); - //! Sample pixels at a given interval + /*! + * Sample pixels at a given interval. + */ virtual void sample(const vpImage &image, bool doNotTrack = false) = 0; + /*! + * Set type of moving-edges display. + * @param select : Display type selector. + */ void setDisplay(vpMeSite::vpMeSiteDisplayType select) { selectDisplay = select; } /*! - Set the initial range. - - \param r : initial range. - */ + * Set the initial range. + * + * \param r : initial range. + */ void setInitRange(const unsigned int &r) { init_range = r; } /*! - Set the mask - - \param mask : Mask. - */ + * Set the mask. + * + * \param mask : Mask. + */ virtual void setMask(const vpImage &mask) { m_mask = &mask; } /*! - Set the moving edges initialisation parameters - - \param p_me : Moving Edges. - */ + * Set the moving edges initialisation parameters. + * + * \param p_me : Moving Edges. + */ void setMe(vpMe *p_me) { this->me = p_me; } /*! - Set the list of moving edges - - \param l : list of Moving Edges. - */ + * Set the list of moving edges. + * + * \param l : list of Moving Edges. + */ void setMeList(const std::list &l) { list = l; } + /*! + * Return the total number of moving-edges. + */ unsigned int totalNumberOfSignal(); - //! Track sampled pixels. + /*! + * Track moving-edges. + * + * \param I : Image. + * + * \exception vpTrackingException::initializationError : Moving edges not initialized. + */ void track(const vpImage &I); //@} diff --git a/modules/tracker/me/include/visp3/me/vpNurbs.h b/modules/tracker/me/include/visp3/me/vpNurbs.h index 533c595a13..16956a1d68 100644 --- a/modules/tracker/me/include/visp3/me/vpNurbs.h +++ b/modules/tracker/me/include/visp3/me/vpNurbs.h @@ -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: * This class implements the Non Uniform Rational B-Spline (NURBS) - * -*****************************************************************************/ + */ #ifndef vpNurbs_H #define vpNurbs_H @@ -94,26 +92,93 @@ class VISP_EXPORT vpNurbs : public vpBSpline { protected: - std::vector weights; // Vector which contains the weights associated - // to each control Points + //! Vector which contains the weights associated to each control Points + std::vector weights; protected: + /*! + * This function is used in the computeCurveDersPoint method. + * + * Compute the kth derivatives of \f$ C(u) \f$ for \f$ k = 0, ... , l_{der} + * \f$. + * + * The formula used is the following : + * + * \f[ C^{(k)}(u) = \sum_{i=0}^n (N_{i,p}^{(k)}(u)Pw_i) \f] + * + * where \f$ i \f$ is the knot interval number in which \f$ u \f$ lies, \f$ p + * \f$ is the degree of the NURBS basis function and \f$ Pw_i = (P_i w_i) \f$ + * contains the control points and the associated weights. + * + * \param l_u : A real number which is between the extremities of the knot + * vector. + * \param l_i : the number of the knot interval in which \f$ l_u \f$ + * lies. + * \param l_p : Degree of the NURBS basis functions. + * \param l_der : The last derivative to be computed. + * \param l_knots : The knot vector. + * \param l_controlPoints : the list of control points. + * \param l_weights : the list of weights. + * + * \return a matrix of size (l_der+1)x3 containing the coordinates \f$ + * C^{(k)}(u) \f$ for \f$ k = 0, ... , l_{der} \f$. The kth derivative is in + * the kth line of the matrix. For each lines the first and the second column + * corresponds to the coordinates (i,j) of the point and the third column + * corresponds to the associated weight. + */ static vpMatrix computeCurveDers(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, std::vector &l_knots, std::vector &l_controlPoints, std::vector &l_weights); + + /*! + * This function is used in the computeCurveDersPoint method. + * + * Compute the kth derivatives of \f$ C(u) \f$ for \f$ k = 0, ... , der \f$. + * + * The formula used is the following : + * + * \f[ C^{(k)}(u) = \sum_{i=0}^n (N_{i,p}^{(k)}(u)Pw_i) \f] + * + * where \f$ i \f$ is the knot interval number in which \f$ u \f$ lies, \f$ p + * \f$ is the degree of the NURBS basis function and \f$ Pw_i = (P_i w_i) \f$ + * contains the control points and the associated weights. + * + * \param u : A real number which is between the extremities of the knot vector + * \param der : The last derivative to be computed. + * + * \return a matrix of size (l_der+1)x3 containing the coordinates \f$ + * C^{(k)}(u) \f$ for \f$ k = 0, ... , der \f$. The kth derivative is in the + * kth line of the matrix. For each lines the first and the second column + * corresponds to the coordinates (i,j) of the point and the third column + * corresponds to the associated weight. + */ vpMatrix computeCurveDers(double u, unsigned int der); public: + /*! + * Basic constructor. + * + * The degree \f$ p \f$ of the NURBS basis functions is set to 3 to + * compute cubic NURBS. + */ vpNurbs(); + + /*! + * Copy constructor. + */ vpNurbs(const vpNurbs &nurbs); - virtual ~vpNurbs(); /*! - Gets all the weights relative to the control points. + * Destructor. + */ + virtual ~vpNurbs(); - \param list [out] : A std::list containing weights relative to the - control points. - */ + /*! + * Gets all the weights relative to the control points. + * + * \param list [out] : A std::list containing weights relative to the + * control points. + */ inline void get_weights(std::list &list) const { list.clear(); @@ -122,10 +187,10 @@ class VISP_EXPORT vpNurbs : public vpBSpline } /*! - Sets all the knots. - - \param list : A std::list containing the value of the knots. - */ + * Sets all the knots. + * + * \param list : A std::list containing the value of the knots. + */ inline void set_weights(const std::list &list) { weights.clear(); @@ -134,43 +199,333 @@ class VISP_EXPORT vpNurbs : public vpBSpline } } + /*! + * Compute the coordinates of a point \f$ C(u) = \frac{\sum_{i=0}^n + * (N_{i,p}(u)w_iP_i)}{\sum_{i=0}^n (N_{i,p}(u)w_i)} \f$ corresponding to the + * knot \f$ u \f$. + * + * \param l_u : A real number which is between the extremities of the knot + * vector. + * \param l_i : the number of the knot interval in which \f$ l_u \f$ + * lies. + * \param l_p : Degree of the NURBS basis functions. + * \param l_knots : The knot vector. + * \param l_controlPoints : the list of control points. + * \param l_weights : the list of weights. + * + * \return The coordinates of a point corresponding to the knot \f$ u \f$. + */ static vpImagePoint computeCurvePoint(double l_u, unsigned int l_i, unsigned int l_p, std::vector &l_knots, std::vector &l_controlPoints, std::vector &l_weights); + + /*! + * Compute the coordinates of a point \f$ C(u) = \frac{\sum_{i=0}^n + * (N_{i,p}(u)w_iP_i)}{\sum_{i=0}^n (N_{i,p}(u)w_i)} \f$ corresponding to the + * knot \f$ u \f$. + * + * \param u : A real number which is between the extremities of the knot vector + * + * return the coordinates of a point corresponding to the knot \f$ u \f$. + */ vpImagePoint computeCurvePoint(double u); + /*! + * Compute the kth derivatives of \f$ C(u) \f$ for \f$ k = 0, ... , l_{der} + * \f$. + * + * To see how the derivatives are computed refers to the Nurbs book. + * + * \param l_u : A real number which is between the extremities of the knot + * vector. + * \param l_i : the number of the knot interval in which \f$ l_u \f$ + * lies. + * \param l_p : Degree of the NURBS basis functions. + * \param l_der : The last derivative to be computed. + * \param l_knots : The knot vector. + * \param l_controlPoints : the list of control points. + * \param l_weights : the list of weights. + * + * \return an array of size l_der+1 containing the coordinates \f$ C^{(k)}(u) + * \f$ for \f$ k = 0, ... , l_{der} \f$. The kth derivative is in the kth cell + * of the array. + */ static vpImagePoint *computeCurveDersPoint(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, std::vector &l_knots, std::vector &l_controlPoints, std::vector &l_weights); + + /*! + * Compute the kth derivatives of \f$ C(u) \f$ for \f$ k = 0, ... , l_{der} + * \f$. + * + * To see how the derivatives are computed refers to the Nurbs book. + * + * \param u : A real number which is between the extremities of the knot vector + * \param der : The last derivative to be computed. + * + * \return an array of size l_der+1 containing the coordinates \f$ C^{(k)}(u) + * \f$ for \f$ k = 0, ... , der \f$. The kth derivative is in the kth cell of + * the array. + */ vpImagePoint *computeCurveDersPoint(double u, unsigned int der); + /*! + * Insert \f$ l_r \f$ times a knot in the \f$ l_k \f$ th interval of the knot + * vector. The inserted knot \f$ l_u \f$ has multiplicity \f$ l_s \f$. + * + * Of course the knot vector changes. But The list of control points and the + * list of the associated weights change too. + * + * \param l_u : A real number which is between the extremities of the knot + * vector and which has to be inserted. + * \param l_k : The number of the knot interval in which \f$ l_u \f$ lies. + * \param l_s : Multiplicity of \f$ l_u \f$ + * \param l_r : Number of times \f$ l_u \f$ has to be inserted. + * \param l_p : Degree of the NURBS basis functions. + * \param l_knots : The knot vector + * \param l_controlPoints : the list of control points. + * \param l_weights : the list of weights. + */ static void curveKnotIns(double l_u, unsigned int l_k, unsigned int l_s, unsigned int l_r, unsigned int l_p, std::vector &l_knots, std::vector &l_controlPoints, std::vector &l_weights); + + /*! + * Insert \f$ r \f$ times a knot in the \f$ k \f$ th interval of the knot + * vector. The inserted knot \f$ u \f$ has multiplicity \f$ s \f$. + * + * Of course the knot vector changes. But The list of control points and the + * list of the associated weights change too. + * + * \param u : A real number which is between the extremities of the knot vector + * and which has to be inserted. + * \param s : Multiplicity of \f$ l_u \f$. + * \param r : Number of times \f$ l_u \f$ has to be inserted. + */ void curveKnotIns(double u, unsigned int s = 0, unsigned int r = 1); + /*! + * Insert \f$ l_r \f$ knots in the knot vector. + * + * Of course the knot vector changes. But The list of control points and the + * list of the associated weights change too. + * + * \param l_x : Several real numbers which are between the extremities of the + * knot vector and which have to be inserted. + * \param l_r : Number of knot in the array \f$ l_x \f$. + * \param l_p : Degree of the NURBS basis functions. + * \param l_knots : The knot vector + * \param l_controlPoints : the list of control points. + * \param l_weights : the list of weights. + */ static void refineKnotVectCurve(double *l_x, unsigned int l_r, unsigned int l_p, std::vector &l_knots, std::vector &l_controlPoints, std::vector &l_weights); + + /*! + * Insert \f$ r \f$ knots in the knot vector. + * + * Of course the knot vector changes. But The list of control points and the + * list of the associated weights change too. + * + * \param x : Several real numbers which are between the extremities of the + * knot vector and which have to be inserted. \param r : Number of knot in the + * array \f$ l_x \f$. + */ void refineKnotVectCurve(double *x, unsigned int r); + /*! + * Remove \f$ l_num \f$ times the knot \f$ l_u \f$ from the knot vector. The + * removed knot \f$ l_u \f$ is the \f$ l_r \f$ th vector in the knot vector. + * + * Of course the knot vector changes. But The list of control points and the + * list of the associated weights change too. + * + * \param l_u : A real number which is between the extremities of the knot + * vector and which has to be removed. + * \param l_r : Index of \f$ l_u \f$ in the knot vector. + * \param l_num : Number of times \f$ l_u \f$ has to be removed. + * \param l_TOL : A parameter which has to be computed. + * \param l_s : Multiplicity of \f$ l_u \f$. + * \param l_p : Degree of the NURBS basis functions. + * \param l_knots : The knot vector + * \param l_controlPoints : the list of control points. + * \param l_weights : the list of weights. + * + * \return The number of time that l_u was removed. + * + * \f$ l_{TOL} = \frac{dw_{min}}{1+|P|_{max}} \f$ + * + * where \f$ w_{min} \f$ is the minimal weight on the original curve, \f$ + * |P|_{max} \f$ is the maximum distance of any point on the original curve + * from the origin and \f$ d \f$ is the desired bound on deviation. + */ static unsigned int removeCurveKnot(double l_u, unsigned int l_r, unsigned int l_num, double l_TOL, unsigned int l_s, unsigned int l_p, std::vector &l_knots, std::vector &l_controlPoints, std::vector &l_weights); + + /*! + * Remove \f$ num \f$ times the knot \f$ u \f$ from the knot vector. The + * removed knot \f$ u \f$ is the \f$ r \f$ th vector in the knot vector. + * + * Of course the knot vector changes. But The list of control points and the + * list of the associated weights change too. + * + * \param l_u : A real number which is between the extremities of the knot vector + * and which has to be removed. + * \param l_r : Index of \f$ l_u \f$ in the knot vector. + * \param l_num : Number of times \f$ l_u \f$ has to be removed. + * \param l_TOL : A parameter which has to be computed. + * + * \return The number of time that l_u was removed. + * + * \f$ TOL = \frac{dw_{min}}{1+|P|_{max}} \f$ + * + * where \f$ w_{min} \f$ is the minimal weight on the original curve, \f$ + * |P|_{max} \f$ is the maximum distance of any point on the original curve + * from the origin and \f$ d \f$ is the desired bound on deviation. + */ unsigned int removeCurveKnot(double l_u, unsigned int l_r, unsigned int l_num, double l_TOL); + /*! + * Method which enables to compute a NURBS curve passing through a set of data + * points. + * + * The result of the method is composed by a knot vector, a set of control + * points and a set of associated weights. + * + * \param l_crossingPoints : The list of data points which have to be interpolated. + * \param l_p : Degree of the NURBS basis functions. This value need to be > 0. + * \param l_knots : The knot vector. + * \param l_controlPoints : The list of control points. + * \param l_weights : the list of weights. + */ static void globalCurveInterp(std::vector &l_crossingPoints, unsigned int l_p, std::vector &l_knots, std::vector &l_controlPoints, std::vector &l_weights); + + /*! + * Method which enables to compute a NURBS curve passing through a set of data + * points. + * + * The result of the method is composed by a knot vector, a set of control + * points and a set of associated weights. + * + * \param l_crossingPoints : The list of data points which have to be + * interpolated. + */ void globalCurveInterp(vpList &l_crossingPoints); + + /*! + * Method which enables to compute a NURBS curve passing through a set of data + * points. + * + * The result of the method is composed by a knot vector, a set of control + * points and a set of associated weights. + * + * \param l_crossingPoints : The list of data points which have to be + * interpolated. + */ void globalCurveInterp(const std::list &l_crossingPoints); + + /*! + * Method which enables to compute a NURBS curve passing through a set of data + * points. + * + * The result of the method is composed by a knot vector, a set of control + * points and a set of associated weights. + * + * \param l_crossingPoints : The list of data points which have to be + * interpolated. + */ void globalCurveInterp(const std::list &l_crossingPoints); + + /*! + * Method which enables to compute a NURBS curve passing through a set of data + * points. + * + * The result of the method is composed by a knot vector, a set of control + * points and a set of associated weights. + */ void globalCurveInterp(); + /*! + * Method which enables to compute a NURBS curve approximating a set of data + * points. + * + * The data points are approximated thanks to a least square method. + * + * The result of the method is composed by a knot vector, a set of control + * points and a set of associated weights. + * + * \param l_crossingPoints : The list of data points which have to be interpolated. + * \param l_p : Degree of the NURBS basis functions. + * \param l_n : The desired number of control points. l_n must be under or equal to the + * number of data points. + * \param l_knots : The knot vector. + * \param l_controlPoints : the list of control points. + * \param l_weights : the list of weights. + */ static void globalCurveApprox(std::vector &l_crossingPoints, unsigned int l_p, unsigned int l_n, std::vector &l_knots, std::vector &l_controlPoints, std::vector &l_weights); + /*! + * Method which enables to compute a NURBS curve approximating a set of + * data points. + * + * The data points are approximated thanks to a least square method. + * + * The result of the method is composed by a knot vector, a set of + * control points and a set of associated weights. + * + * \param l_crossingPoints : The list of data points which have to be + * interpolated. + * + * \param n : The desired number of control points. This parameter \e n + * must be under or equal to the number of data points. + */ void globalCurveApprox(vpList &l_crossingPoints, unsigned int n); + + /*! + * Method which enables to compute a NURBS curve approximating a set of data + * points. + * + * The data points are approximated thanks to a least square method. + * + * The result of the method is composed by a knot vector, a set of control + * points and a set of associated weights. + * + * \param l_crossingPoints : The list of data points which have to be + * interpolated. + * \param n : The desired number of control points. The parameter + * \e n must be under or equal to the number of data points. + */ void globalCurveApprox(const std::list &l_crossingPoints, unsigned int n); + + /*! + * Method which enables to compute a NURBS curve approximating a set of + * data points. + * + * The data points are approximated thanks to a least square method. + * + * The result of the method is composed by a knot vector, a set of + * control points and a set of associated weights. + * + * \param l_crossingPoints : The list of data points which have to be + * interpolated. + * + * \param n : The desired number of control points. This parameter \e n + * must be under or equal to the number of data points. + */ void globalCurveApprox(const std::list &l_crossingPoints, unsigned int n); + + /*! + * Method which enables to compute a NURBS curve approximating a set of data + * points. + * + * The data points are approximated thanks to a least square method. + * + * The result of the method is composed by a knot vector, a set of control + * points and a set of associated weights. + */ void globalCurveApprox(unsigned int n); }; diff --git a/modules/tracker/me/src/moving-edges/vpMe.cpp b/modules/tracker/me/src/moving-edges/vpMe.cpp index 10a6afc0d3..f6864abc4d 100644 --- a/modules/tracker/me/src/moving-edges/vpMe.cpp +++ b/modules/tracker/me/src/moving-edges/vpMe.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: * Moving edges. - * -*****************************************************************************/ + */ /*! \file vpMe.cpp @@ -42,15 +40,17 @@ #include #include #include -#ifndef DOXYGEN_SHOULD_SKIP_THIS -struct point +#ifndef DOXYGEN_SHOULD_SKIP_THIS +namespace +{ +struct vpPoint2D_t { double x; double y; }; -struct droite +struct vpDroite2D_t { double a; double b; @@ -64,9 +64,9 @@ template inline void permute(Type &a, Type &b) b = t; } -static droite droite_cartesienne(point P, point Q) +static vpDroite2D_t droite_cartesienne(vpPoint2D_t P, vpPoint2D_t Q) { - droite PQ; + vpDroite2D_t PQ; PQ.a = P.y - Q.y; PQ.b = Q.x - P.x; @@ -75,9 +75,9 @@ static droite droite_cartesienne(point P, point Q) return (PQ); } -static point point_intersection(droite D1, droite D2) +static vpPoint2D_t point_intersection(vpDroite2D_t D1, vpDroite2D_t D2) { - point I; + vpPoint2D_t I; double det; // determinant des 2 vect.normaux det = (D1.a * D2.b - D2.a * D1.b); // interdit D1,D2 paralleles @@ -87,7 +87,7 @@ static point point_intersection(droite D1, droite D2) return (I); } -static void recale(point &P, double Xmin, double Ymin, double Xmax, double Ymax) +static void recale(vpPoint2D_t &P, double Xmin, double Ymin, double Xmax, double Ymax) { if (vpMath::equal(P.x, Xmin)) P.x = Xmin; // a peu pres => exactement ! @@ -100,9 +100,9 @@ static void recale(point &P, double Xmin, double Ymin, double Xmax, double Ymax) P.y = Ymax; } -static void permute(point &A, point &B) +static void permute(vpPoint2D_t &A, vpPoint2D_t &B) { - point C; + vpPoint2D_t C; if (A.x > B.x) // fonction sans doute a tester... { @@ -113,10 +113,10 @@ static void permute(point &A, point &B) } // vrai si partie visible -static bool clipping(point A, point B, double Xmin, double Ymin, double Xmax, double Ymax, point &Ac, - point &Bc) // resultat: A,B clippes +static bool clipping(vpPoint2D_t A, vpPoint2D_t B, double Xmin, double Ymin, double Xmax, double Ymax, vpPoint2D_t &Ac, + vpPoint2D_t &Bc) // resultat: A,B clippes { - droite AB, D[4]; + vpDroite2D_t AB, D[4]; D[0].a = 1; D[0].b = 0; D[0].c = -Xmin; @@ -130,7 +130,7 @@ static bool clipping(point A, point B, double Xmin, double Ymin, double Xmax, do D[3].b = 1; D[3].c = -Ymax; - point P[2]; + vpPoint2D_t P[2]; P[0] = A; P[1] = B; int code_P[2], // codes de P[n] @@ -205,7 +205,7 @@ static bool clipping(point A, point B, double Xmin, double Ymin, double Xmax, do // 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 S_relative(point P, point Q, double Xmin, double Ymin, double Xmax, double Ymax) +static double S_relative(vpPoint2D_t P, vpPoint2D_t Q, double Xmin, double Ymin, double Xmax, double Ymax) { if (Q.x < P.x) // tri le couple de points @@ -267,7 +267,7 @@ static void calcul_masques(vpColVector &angle, // definitions des angles theta { unsigned int i, j; double X, Y, moitie = ((double)n) / 2.0; // moitie REELLE du masque - point P1, Q1, P, Q; // clippe Droite(theta) P1,Q1 -> P,Q + vpPoint2D_t P1, Q1, P, Q; // clippe Droite(theta) P1,Q1 -> P,Q double v; // ponderation de M(i,j) // For a mask of size nxn, normalization given by n*trunc(n/2.0) @@ -323,14 +323,9 @@ static void calcul_masques(vpColVector &angle, // definitions des angles theta } } } - +} #endif -/*! - Initialise the array of matrices with the defined size and the number of - matrices to create. - -*/ void vpMe::initMask() { if (mask != NULL) @@ -386,7 +381,6 @@ vpMe::vpMe(const vpMe &me) *this = me; } -//! Copy operator. vpMe &vpMe::operator=(const vpMe &me) { if (mask != NULL) { @@ -414,7 +408,6 @@ vpMe &vpMe::operator=(const vpMe &me) } #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) -//! Move operator. vpMe &vpMe::operator=(const vpMe &&me) { if (mask != NULL) { diff --git a/modules/tracker/me/src/moving-edges/vpMeEllipse.cpp b/modules/tracker/me/src/moving-edges/vpMeEllipse.cpp index 4b61a493d2..7cc38a6181 100644 --- a/modules/tracker/me/src/moving-edges/vpMeEllipse.cpp +++ b/modules/tracker/me/src/moving-edges/vpMeEllipse.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -27,8 +26,7 @@ * * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - * -*****************************************************************************/ + */ #include // std::fabs #include // numeric_limits @@ -42,9 +40,7 @@ #include #include -/*! - Basic constructor that calls the constructor of the class vpMeTracker. -*/ + vpMeEllipse::vpMeEllipse() : K(), iPc(), a(0.), b(0.), e(0.), iP1(), iP2(), alpha1(0), alpha2(2 * M_PI), ce(0.), se(0.), angle(), m00(0.), #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS @@ -60,9 +56,6 @@ vpMeEllipse::vpMeEllipse() iP2.set_ij(0, 0); } -/*! - Copy constructor. -*/ vpMeEllipse::vpMeEllipse(const vpMeEllipse &me_ellipse) : vpMeTracker(me_ellipse), K(me_ellipse.K), iPc(me_ellipse.iPc), a(me_ellipse.a), b(me_ellipse.b), e(me_ellipse.e), iP1(me_ellipse.iP1), iP2(me_ellipse.iP2), alpha1(me_ellipse.alpha1), alpha2(me_ellipse.alpha2), ce(me_ellipse.ce), @@ -79,22 +72,12 @@ vpMeEllipse::vpMeEllipse(const vpMeEllipse &me_ellipse) m_trackCircle(me_ellipse.m_trackCircle), m_trackArc(me_ellipse.m_trackArc) { } -/*! - Basic destructor. -*/ vpMeEllipse::~vpMeEllipse() { list.clear(); angle.clear(); } -/*! - Computes the \f$ \theta \f$ angle that represents the angle between the - tangent to the curve and the u axis. This angle is used for tracking the - vpMeSite. - - \param iP : The point belonging to the ellipse where the angle is computed. -*/ double vpMeEllipse::computeTheta(const vpImagePoint &iP) const { double u = iP.get_u(); @@ -103,13 +86,6 @@ double vpMeEllipse::computeTheta(const vpImagePoint &iP) const return (computeTheta(u, v)); } -/*! - Computes the \f$ \theta \f$ angle that represents the angle between the - tangent to the curve and the u axis. This angle is used for tracking the - vpMeSite. - - \param u,v : The point belonging to the ellipse where the angle is computed. -*/ double vpMeEllipse::computeTheta(double u, double v) const { double A = K[0] * u + K[2] * v + K[3]; @@ -122,11 +98,6 @@ double vpMeEllipse::computeTheta(double u, double v) const return (theta); } -/*! - Compute the \f$ theta \f$ angle for each vpMeSite. - - \note The \f$ theta \f$ angle is useful during the tracking part. -*/ void vpMeEllipse::updateTheta() { vpMeSite p_me; @@ -140,13 +111,6 @@ void vpMeEllipse::updateTheta() } } -/*! - Compute the coordinates of a point on an ellipse from its angle with respect - to the main orientation of the ellipse. - - \param angle : Angle on the ellipse with respect to its major axis. - \param iP : Image point on the ellipse. -*/ void vpMeEllipse::computePointOnEllipse(const double angle, vpImagePoint &iP) { // Two versions are available. If you change from one version to the other @@ -180,11 +144,6 @@ void vpMeEllipse::computePointOnEllipse(const double angle, vpImagePoint &iP) iP.set_uv(m_uc + ce * u - se * v, m_vc + se * u + ce * v); } -/*! - Compute the angle of a point on the ellipse wrt the ellipse major axis. - \param pt : Image point on the ellipse. - \return The computed angle. -*/ double vpMeEllipse::computeAngleOnEllipse(const vpImagePoint &pt) const { // Two versions are available. If you change from one version to the other @@ -225,12 +184,6 @@ double vpMeEllipse::computeAngleOnEllipse(const vpImagePoint &pt) const return (angle); } -/*! - Computes the length of the semi major axis \f$ a \f$, the length of the - semi minor axis \f$ b \f$, and \f$ e \f$ that is the angle - made by the major axis and the u axis of the image frame \f$ (u,v) \f$. - They are computed from the normalized moments $ \f$ n_{ij} \f$. -*/ void vpMeEllipse::computeAbeFromNij() { double num = m_n20 - m_n02; @@ -253,12 +206,6 @@ void vpMeEllipse::computeAbeFromNij() } } -/*! - Computes the parameters \f$ K = {K_0, ..., K_5} \f$ from the center of - the ellipse and the normalized moments \f$ n_{ij} \f$. The parameters - \f$ K \f$ are such that \f$ K0 = n02, K1 = n20 \f$, etc. as in Eq (25) - of Chaumette 2004 TRO paper. -*/ void vpMeEllipse::computeKiFromNij() { K[0] = m_n02; @@ -269,11 +216,6 @@ void vpMeEllipse::computeKiFromNij() 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); } -/*! - Computes the normalized moments \f$ n_{ij} \f$ from the \f$ A, B, E \f$ - parameters as in Eq (24) of Chaumette 2004 TRO paper after simplifications - to deal with the case cos(e) = 0.0 -*/ void vpMeEllipse::computeNijFromAbe() { m_n20 = 0.25 * (a * a * ce * ce + b * b * se * se); @@ -281,15 +223,6 @@ void vpMeEllipse::computeNijFromAbe() m_n02 = 0.25 * (a * a * se * se + b * b * ce * ce); } -/*! - Computes the coordinates of the ellipse center, the normalized - moments \f$ n_{ij} \f$, the length of the semi major axis \f$ a \f$, the - length of the semi minor axis \f$ b \f$, and \f$ e \f$ that is the angle - made by the major axis and the u axis of the image frame \f$ (u,v) \f$. - - All those computations are made from the parameters \f$ K ={K_0, ..., K_5} \f$ - so that \f$ K_0 u^2 + K1 v^2 + 2 K2 u v + 2 K3 u + 2 K4 v + K5 = 0 \f$. -*/ void vpMeEllipse::getParameters() { // Equations below from Chaumette PhD and TRO 2004 paper @@ -319,10 +252,6 @@ void vpMeEllipse::getParameters() } } -/*! - Print the parameters \f$ K = {K_0, ..., K_5} \f$, the coordinates of the - ellipse center, the normalized moments, and the A, B, E parameters. -*/ void vpMeEllipse::printParameters() const { std::cout << "K :" << K.t() << std::endl; @@ -331,18 +260,6 @@ void vpMeEllipse::printParameters() const std::cout << "A = " << a << ", B = " << b << ", E (dg) " << vpMath::deg(e) << std::endl; } -/*! - Construct a list of vpMeSite moving edges at a particular sampling - step between the two extremities. The two extremities are defined by - the points with the smallest and the biggest \f$ alpha \f$ angle. - - \param I : Image in which the ellipse appears. - \param doNotTrack : If true, moving-edges are not tracked. - - \exception vpTrackingException::initializationError : Moving edges not - initialized. - -*/ void vpMeEllipse::sample(const vpImage &I, bool doNotTrack) { // Warning: similar code in vpMbtMeEllipse::sample() @@ -404,17 +321,6 @@ void vpMeEllipse::sample(const vpImage &I, bool doNotTrack) } } -/*! - 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. - - \param I : Image in which the ellipse appears. - - \return The function returns the number of points added to the list. - - \exception vpTrackingException::initializationError : Moving edges not - initialized. -*/ unsigned int vpMeEllipse::plugHoles(const vpImage &I) { if (!me) { @@ -632,13 +538,6 @@ unsigned int vpMeEllipse::plugHoles(const vpImage &I) return nb_pts_added; } -/*! - Least squares method to compute the circle/ ellipse to which the points belong. - - \param I : Image in which the circle/ellipse appears (useful just to get - its number of rows and columns... - \param iP : A vector of points belonging to the circle/ellipse. -*/ void vpMeEllipse::leastSquare(const vpImage &I, const std::vector &iP) { double um = I.getWidth() / 2.; @@ -721,14 +620,6 @@ void vpMeEllipse::leastSquare(const vpImage &I, const std::vector getParameters(); } -/*! - Robust least squares method to compute the ellipse to which the vpMeSite - belong. Manage also the lists of vpMeSite and corresponding angles, - and update the expected density of points. - - \param I : Image where tracking is done (useful just to get its number - of rows and columns... -*/ unsigned int vpMeEllipse::leastSquareRobust(const vpImage &I) { double um = I.getWidth() / 2.; @@ -1106,34 +997,11 @@ unsigned int vpMeEllipse::leastSquareRobust(const vpImage &I) return(numberOfGoodPoints); } -/*! - Display the ellipse or arc of ellipse - - \warning To effectively display the ellipse a call to - vpDisplay::flush() is needed. - - \param I : Image in which the ellipse appears. - \param col : Color of the displayed ellipse. - \param thickness : Thickness of the drawing. - */ void vpMeEllipse::display(const vpImage &I, const vpColor &col, unsigned int thickness) { vpMeEllipse::displayEllipse(I, iPc, a, b, e, alpha1, alpha2, col, thickness); } -/*! - Initialize the tracking of an ellipse or an arc of an ellipse when \e trackArc is set to true. - Ask the user to click on five points located on the ellipse to be tracked. - - \warning The points should be selected as far as possible from each other. - When an arc of an ellipse is tracked, it is recommended to select the 5 points clockwise. - - \param I : Image in which the ellipse appears. - \param trackCircle : When true, track a circle, when false track an ellipse. - \param trackArc : When true track an arc of the ellipse/circle. In that case, first and - last points specify the extremities of the arc (clockwise). - When false track the complete ellipse/circle. -*/ void vpMeEllipse::initTracking(const vpImage &I, bool trackCircle, bool trackArc) { unsigned int n = 5; // by default, 5 points for an ellipse @@ -1172,23 +1040,6 @@ void vpMeEllipse::initTracking(const vpImage &I, bool trackCircle initTracking(I, iP, trackCircle, trackArc); } -/*! - Initialize the tracking of an ellipse/circle or an arc of an ellipse/circle when \e trackArc is set to true. - The ellipse/circle is defined thanks to a vector of image points. - - \warning It is mandatory to use at least five image points to estimate the - ellipse parameters while three points are needed to estimate the circle parameters. - \warning The image points should be selected as far as possible from each other. - When an arc of an ellipse/circle is tracked, it is recommended to select the 5/3 points clockwise. - - \param I : Image in which the ellipse/circle appears. - \param iP : A vector of image points belonging to the ellipse/circle edge used to - initialize the tracking. - \param trackCircle : When true, track a circle, when false track an ellipse. - \param trackArc : When true track an arc of the ellipse/circle. In that case, first and - last points specify the extremities of the arc (clockwise). - When false track the complete ellipse/circle. -*/ void vpMeEllipse::initTracking(const vpImage &I, const std::vector &iP, bool trackCircle, bool trackArc) { @@ -1222,21 +1073,6 @@ void vpMeEllipse::initTracking(const vpImage &I, const std::vecto vpDisplay::flush(I); } -/*! - Initialize the tracking of an ellipse/circle or an arc of an ellipse/circle when arc extremities are given. - The ellipse/circle is defined by the vector containing the coordinates of its center and the three second order - centered normalized moments \f$ n_ij \f$. Without setting the arc extremities with - parameters \e pt1 and \e pt2, the complete ellipse/circle is considered. When extremities - are set, we consider an ellipse/circle arc defined clockwise from first extremity to second extremity. - - \param I : Image in which the ellipse appears. - \param param : Vector with the five parameters \f$(u_c, v_c, n_{20}, n_{11}, n_{02})\f$ defining the ellipse - (expressed in pixels). - \param pt1 : Image point defining the first extremity of the arc or NULL to track a complete ellipse. - \param pt2 : Image point defining the second extremity of the arc or NULL to track a complete ellipse. - \param trackCircle : When true enable tracking of a circle, when false the tracking of an ellipse. - -*/ void vpMeEllipse::initTracking(const vpImage &I, const vpColVector ¶m, vpImagePoint *pt1, const vpImagePoint *pt2, bool trackCircle) { @@ -1531,65 +1367,17 @@ void vpMeEllipse::display(const vpImage &I, const vpImagePoint ¢er, } #endif // Deprecated -/*! - Display the ellipse or the arc of ellipse thanks to the ellipse parameters. - - \param I : The image used as background. - - \param center : Center of the ellipse. - - \param A : Semi major axis of the ellipse. - - \param B : Semi minor axis of the ellipse. - - \param E : Angle made by the major axis and the u axis of the image frame - \f$ (u,v) \f$ (in rad). - - \param smallalpha : Smallest \f$ alpha \f$ angle in rad (0 for a complete ellipse). - - \param highalpha : Highest \f$ alpha \f$ angle in rad (2 \f$ \Pi \f$ for a complete ellipse). - - \param color : Color used to display the ellipse. - - \param thickness : Thickness of the drawings. - \sa vpDisplay::displayEllipse() -*/ void vpMeEllipse::displayEllipse(const vpImage &I, const vpImagePoint ¢er, const double &A, - const double &B, const double &E, const double &smallalpha, const double &highalpha, - const vpColor &color, unsigned int thickness) + const double &B, const double &E, const double &smallalpha, const double &highalpha, + const vpColor &color, unsigned int thickness) { vpDisplay::displayEllipse(I, center, A, B, E, smallalpha, highalpha, false, color, thickness, true, true); } -/*! - - Display the ellipse or the arc of ellipse thanks to the ellipse parameters. - - \param I : The image used as background. - - \param center : Center of the ellipse - - \param A : Semimajor axis of the ellipse. - - \param B : Semiminor axis of the ellipse. - - \param E : Angle made by the major axis and the u axis of the image frame - \f$ (u,v) \f$ (in rad) - - \param smallalpha : Smallest \f$ alpha \f$ angle in rad (0 for a complete ellipse) - - \param highalpha : Highest \f$ alpha \f$ angle in rad (\f$ 2 \Pi \f$ for a complete ellipse) - - \param color : Color used to display th lines. - - \param thickness : Thickness of the drawings. - - \sa vpDisplay::displayEllipse() -*/ void vpMeEllipse::displayEllipse(const vpImage &I, const vpImagePoint ¢er, const double &A, const double &B, - const double &E, const double &smallalpha, const double &highalpha, - const vpColor &color, unsigned int thickness) + const double &E, const double &smallalpha, const double &highalpha, + const vpColor &color, unsigned int thickness) { vpDisplay::displayEllipse(I, center, A, B, E, smallalpha, highalpha, false, color, thickness, true, true); } diff --git a/modules/tracker/me/src/moving-edges/vpMeLine.cpp b/modules/tracker/me/src/moving-edges/vpMeLine.cpp index 3a4bd73464..db84354203 100644 --- a/modules/tracker/me/src/moving-edges/vpMeLine.cpp +++ b/modules/tracker/me/src/moving-edges/vpMeLine.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: * Moving edges. - * -*****************************************************************************/ + */ /*! \file vpMeLine.cpp @@ -88,20 +86,11 @@ static void project(double a, double b, double c, double i, double j, double &ip } } -/*! - - Basic constructor that calls the constructor of the class vpMeTracker. - -*/ vpMeLine::vpMeLine() : rho(0.), theta(0.), delta(0.), delta_1(0.), angle(0.), angle_1(90), sign(1), _useIntensityForRho(true), a(0.), b(0.), c(0.) { } -/*! - Copy constructor. - -*/ vpMeLine::vpMeLine(const vpMeLine &meline) : vpMeTracker(meline), rho(0.), theta(0.), delta(0.), delta_1(0.), angle(0.), angle_1(90), sign(1), _useIntensityForRho(true), a(0.), b(0.), c(0.) @@ -123,25 +112,8 @@ vpMeLine::vpMeLine(const vpMeLine &meline) PExt[1] = meline.PExt[1]; } -/*! - - Basic destructor. - -*/ vpMeLine::~vpMeLine() { list.clear(); } -/*! - - Construct a list of vpMeSite moving edges at a particular sampling - step between the two extremities of the line. - - \param I : Image in which the line appears. - \param doNotTrack : Inherited parameter, not used. - - \exception vpTrackingException::initializationError : Moving edges not - initialized. - -*/ void vpMeLine::sample(const vpImage &I, bool doNotTrack) { (void)doNotTrack; @@ -206,32 +178,11 @@ void vpMeLine::sample(const vpImage &I, bool doNotTrack) vpCDEBUG(1) << n_sample << " point inserted in the list " << std::endl; } -/*! - Display line. - - \warning To effectively display the line a call to - vpDisplay::flush() is needed. - - \param I : Image in which the line appears. - - \param color : Color of the displayed line. Note that a moving edge - that is considered as an outlier is displayed in green. - - \param thickness : Drawings thickness. - - */ void vpMeLine::display(const vpImage &I, const vpColor &color, unsigned int thickness) { vpMeLine::displayLine(I, PExt[0], PExt[1], list, a, b, c, color, thickness); } -/*! - - Initialization of the tracking. Ask the user to click on two points - from the line to track. - - \param I : Image in which the line appears. -*/ void vpMeLine::initTracking(const vpImage &I) { vpImagePoint ip1, ip2; @@ -258,12 +209,6 @@ void vpMeLine::initTracking(const vpImage &I) } } -/*! - - Least squares method used to make the tracking more robust. It - ensures that the points taken into account to compute the right - equation belong to the line. -*/ void vpMeLine::leastSquare() { vpMatrix A(numberOfSignal(), 2); @@ -414,15 +359,6 @@ void vpMeLine::leastSquare() normalizeAngle(delta); } -/*! - - Initialization of the tracking. The line is defined thanks to the - coordinates of two points. - - \param I : Image in which the line appears. - \param ip1 : Coordinates of the first point. - \param ip2 : Coordinates of the second point. -*/ void vpMeLine::initTracking(const vpImage &I, const vpImagePoint &ip1, const vpImagePoint &ip2) { vpCDEBUG(1) << " begin vpMeLine::initTracking()" << std::endl; @@ -474,9 +410,6 @@ void vpMeLine::initTracking(const vpImage &I, const vpImagePoint vpCDEBUG(1) << " end vpMeLine::initTracking()" << std::endl; } -/*! - Suppression of the points which belong no more to the line. -*/ void vpMeLine::suppressPoints() { // Loop through list of sites to track @@ -490,9 +423,6 @@ void vpMeLine::suppressPoints() } } -/*! - Seek in the list of available points the two extremities of the line. -*/ void vpMeLine::setExtremities() { double imin = +1e6; @@ -539,17 +469,6 @@ void vpMeLine::setExtremities() } } -/*! - - Seek along the line defined by its equation, the two extremities of - the line. This function is useful in case of translation of the - line. - - \param I : Image in which the line appears. - - \exception vpTrackingException::initializationError : Moving edges not - initialized. -*/ void vpMeLine::seekExtremities(const vpImage &I) { vpCDEBUG(1) << "begin vpMeLine::sample() : " << std::endl; @@ -657,17 +576,6 @@ void vpMeLine::seekExtremities(const vpImage &I) vpCDEBUG(1) << n_sample << " point inserted in the list " << std::endl; } -/*! - - Resample the line if the number of sample is less than 80% of the - expected value. - - \note The expected value is computed thanks to the length of the - line and the parameter which indicates the number of pixel between - two points (vpMe::sample_step). - - \param I : Image in which the line appears. -*/ void vpMeLine::reSample(const vpImage &I) { double i1, j1, i2, j2; @@ -703,10 +611,6 @@ void vpMeLine::reSample(const vpImage &I) } } -/*! - - Set the alpha value of the different vpMeSite to the value of delta. -*/ void vpMeLine::updateDelta() { vpMeSite p_me; @@ -742,12 +646,6 @@ void vpMeLine::updateDelta() delta_1 = delta; } -/*! - - Track the line in the image I. - - \param I : Image in which the line appears. -*/ void vpMeLine::track(const vpImage &I) { vpCDEBUG(1) << "begin vpMeLine::track()" << std::endl; @@ -819,12 +717,6 @@ void vpMeLine::update_indices(double theta, int i, int j, int incr, int &i1, int j2 = (int)(j - sin(theta) * incr); } -/*! - - Compute the two parameters \f$(\rho, \theta)\f$ of the line. - - \param I : Image in which the line appears. -*/ void vpMeLine::computeRhoTheta(const vpImage &I) { // rho = -c ; @@ -838,25 +730,6 @@ void vpMeLine::computeRhoTheta(const vpImage &I) theta += M_PI; if (_useIntensityForRho) { - - /* while(theta < -M_PI) theta += 2*M_PI ; - while(theta >= M_PI) theta -= 2*M_PI ; - - // If theta is between -90 and -180 get the equivalent - // between 0 and 90 - if(theta <-M_PI/2) - { - theta += M_PI ; - rho *= -1 ; - } - // If theta is between 90 and 180 get the equivalent - // between 0 and -90 - if(theta >M_PI/2) - { - theta -= M_PI ; - rho *= -1 ; - } - */ // convention pour choisir le signe de rho int i, j; i = vpMath::round((PExt[0].ifloat + PExt[1].ifloat) / 2); @@ -949,30 +822,10 @@ void vpMeLine::computeRhoTheta(const vpImage &I) } } -/*! - - Get the value of \f$\rho\f$, the distance between the origin and the - point on the line with belong to the normal to the line crossing - the origin. - - Depending on the convention described at the beginning of this - class, \f$\rho\f$ is signed. - -*/ double vpMeLine::getRho() const { return rho; } -/*! - Get the value of the angle \f$\theta\f$. -*/ double vpMeLine::getTheta() const { return theta; } -/*! - - Get the extremities of the line. - - \param ip1 : Coordinates of the first extremity. - \param ip2 : Coordinates of the second extremity. -*/ void vpMeLine::getExtremities(vpImagePoint &ip1, vpImagePoint &ip2) { /*Return the coordinates of the extremities of the line*/ @@ -982,18 +835,6 @@ void vpMeLine::getExtremities(vpImagePoint &ip1, vpImagePoint &ip2) ip2.set_j(PExt[1].jfloat); } -/*! - - Computes the intersection point of two lines. The result is given in - the (i,j) frame. - - \param line1 : The first line. - \param line2 : The second line. - \param ip : The coordinates of the intersection point. - - \return Returns a boolean value which depends on the computation - success. True means that the computation ends successfully. -*/ bool vpMeLine::intersection(const vpMeLine &line1, const vpMeLine &line2, vpImagePoint &ip) { double a1 = line1.a; @@ -1056,9 +897,9 @@ bool vpMeLine::intersection(const vpMeLine &line1, const vpMeLine &line2, vpImag \param I : The image used as background. - \param PExt1 : First extrimity + \param PExt1 : First extremity - \param PExt2 : Second extrimity + \param PExt2 : Second extremity \param A : Parameter a of the line equation a*i + b*j + c = 0 @@ -1083,9 +924,9 @@ void vpMeLine::display(const vpImage &I, const vpMeSite &PExt1, c \param I : The image used as background. - \param PExt1 : First extrimity + \param PExt1 : First extremity - \param PExt2 : Second extrimity + \param PExt2 : Second extremity \param A : Parameter a of the line equation a*i + b*j + c = 0 @@ -1110,9 +951,9 @@ void vpMeLine::display(const vpImage &I, const vpMeSite &PExt1, const vp \param I : The image used as background. - \param PExt1 : First extrimity + \param PExt1 : First extremity - \param PExt2 : Second extrimity + \param PExt2 : Second extremity \param site_list : vpMeSite list @@ -1140,9 +981,9 @@ void vpMeLine::display(const vpImage &I, const vpMeSite &PExt1, c \param I : The image used as background. - \param PExt1 : First extrimity + \param PExt1 : First extremity - \param PExt2 : Second extrimity + \param PExt2 : Second extremity \param site_list : vpMeSite list @@ -1165,26 +1006,6 @@ void vpMeLine::display(const vpImage &I, const vpMeSite &PExt1, const vp } #endif // Deprecated -/*! - Display of a moving line thanks to its equation parameters and its - extremities. - - \param I : The image used as background. - - \param PExt1 : First extrimity - - \param PExt2 : Second extrimity - - \param A : Parameter a of the line equation a*i + b*j + c = 0 - - \param B : Parameter b of the line equation a*i + b*j + c = 0 - - \param C : Parameter c of the line equation a*i + b*j + c = 0 - - \param color : Color used to display the line. - - \param thickness : Thickness of the line. -*/ void vpMeLine::displayLine(const vpImage &I, const vpMeSite &PExt1, const vpMeSite &PExt2, const double &A, const double &B, const double &C, const vpColor &color, unsigned int thickness) { @@ -1226,28 +1047,8 @@ void vpMeLine::displayLine(const vpImage &I, const vpMeSite &PExt vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness); } -/*! - Display of a moving line thanks to its equation parameters and its - extremities. - - \param I : The image used as background. - - \param PExt1 : First extrimity - - \param PExt2 : Second extrimity - - \param A : Parameter a of the line equation a*i + b*j + c = 0 - - \param B : Parameter b of the line equation a*i + b*j + c = 0 - - \param C : Parameter c of the line equation a*i + b*j + c = 0 - - \param color : Color used to display the line. - - \param thickness : Thickness of the line. -*/ void vpMeLine::displayLine(const vpImage &I, const vpMeSite &PExt1, const vpMeSite &PExt2, const double &A, - const double &B, const double &C, const vpColor &color, unsigned int thickness) + const double &B, const double &C, const vpColor &color, unsigned int thickness) { vpImagePoint ip1, ip2; @@ -1287,31 +1088,9 @@ void vpMeLine::displayLine(const vpImage &I, const vpMeSite &PExt1, cons vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness); } -/*! - Display of a moving line thanks to its equation parameters and its - extremities with all the site list. - - \param I : The image used as background. - - \param PExt1 : First extrimity - - \param PExt2 : Second extrimity - - \param site_list : vpMeSite list - - \param A : Parameter a of the line equation a*i + b*j + c = 0 - - \param B : Parameter b of the line equation a*i + b*j + c = 0 - - \param C : Parameter c of the line equation a*i + b*j + c = 0 - - \param color : Color used to display the line. - - \param thickness : Thickness of the line. -*/ void vpMeLine::displayLine(const vpImage &I, const vpMeSite &PExt1, const vpMeSite &PExt2, - const std::list &site_list, const double &A, const double &B, const double &C, - const vpColor &color, unsigned int thickness) + const std::list &site_list, const double &A, const double &B, const double &C, + const vpColor &color, unsigned int thickness) { vpImagePoint ip; @@ -1364,31 +1143,9 @@ void vpMeLine::displayLine(const vpImage &I, const vpMeSite &PExt vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness); } -/*! - Display of a moving line thanks to its equation parameters and its - extremities with all the site list. - - \param I : The image used as background. - - \param PExt1 : First extrimity - - \param PExt2 : Second extrimity - - \param site_list : vpMeSite list - - \param A : Parameter a of the line equation a*i + b*j + c = 0 - - \param B : Parameter b of the line equation a*i + b*j + c = 0 - - \param C : Parameter c of the line equation a*i + b*j + c = 0 - - \param color : Color used to display the line. - - \param thickness : Thickness of the line. -*/ void vpMeLine::displayLine(const vpImage &I, const vpMeSite &PExt1, const vpMeSite &PExt2, - const std::list &site_list, const double &A, const double &B, const double &C, - const vpColor &color, unsigned int thickness) + const std::list &site_list, const double &A, const double &B, const double &C, + const vpColor &color, unsigned int thickness) { vpImagePoint ip; diff --git a/modules/tracker/me/src/moving-edges/vpMeNurbs.cpp b/modules/tracker/me/src/moving-edges/vpMeNurbs.cpp index c000eb1587..54ae6dbe26 100644 --- a/modules/tracker/me/src/moving-edges/vpMeNurbs.cpp +++ b/modules/tracker/me/src/moving-edges/vpMeNurbs.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: * Moving edges. - * -*****************************************************************************/ + */ /*! \file vpMeNurbs.cpp @@ -203,17 +201,11 @@ bool findCenterPoint(std::list *ip_edges_list) /***************************************/ -/*! - Basic constructor that calls the constructor of the class vpMeTracker. -*/ vpMeNurbs::vpMeNurbs() : nurbs(), dist(0.), nbControlPoints(20), beginPtFound(0), endPtFound(0), enableCannyDetection(false), cannyTh1(100.), cannyTh2(200.) { } -/*! - Copy constructor. -*/ vpMeNurbs::vpMeNurbs(const vpMeNurbs &menurbs) : vpMeTracker(menurbs), nurbs(menurbs.nurbs), dist(0.), nbControlPoints(20), beginPtFound(0), endPtFound(0), enableCannyDetection(false), cannyTh1(100.f), cannyTh2(200.f) @@ -227,17 +219,8 @@ vpMeNurbs::vpMeNurbs(const vpMeNurbs &menurbs) cannyTh2 = menurbs.cannyTh2; } -/*! - Basic destructor. -*/ vpMeNurbs::~vpMeNurbs() { } -/*! - Initialization of the tracking. Ask the user to click left on several points - along the edge to track and click right at the end. - - \param I : Image in which the edge appears. -*/ void vpMeNurbs::initTracking(const vpImage &I) { std::list ptList; @@ -260,13 +243,6 @@ void vpMeNurbs::initTracking(const vpImage &I) throw(vpException(vpException::notInitialized, "Not enough points to initialize the Nurbs")); } -/*! - Initialization of the tracking. The Nurbs is initialized thanks to the - list of vpImagePoint. - - \param I : Image in which the edge appears. - \param ptList : List of point to initialize the Nurbs. -*/ void vpMeNurbs::initTracking(const vpImage &I, const std::list &ptList) { nurbs.globalCurveInterp(ptList); @@ -277,13 +253,6 @@ void vpMeNurbs::initTracking(const vpImage &I, const std::list &I, bool doNotTrack) { (void)doNotTrack; @@ -319,12 +288,6 @@ void vpMeNurbs::sample(const vpImage &I, bool doNotTrack) delete[] pt; } -/*! - Suppression of the points which: - - - belong no more to the edge. - - which are to closed to another point. -*/ void vpMeNurbs::suppressPoints() { for (std::list::iterator it = list.begin(); it != list.end();) { @@ -338,10 +301,6 @@ void vpMeNurbs::suppressPoints() } } -/*! - Set the alpha value (normal to the edge at this point) - of the different vpMeSite to a value computed thanks to the nurbs. -*/ void vpMeNurbs::updateDelta() { double u = 0.0; @@ -379,13 +338,6 @@ void vpMeNurbs::updateDelta() delete[] der; } -/*! - Seek along the edge defined by the nurbs, the two extremities of - the edge. This function is useful in case of translation of the - edge. - - \param I : Image in which the edge appears. -*/ void vpMeNurbs::seekExtremities(const vpImage &I) { int rows = (int)I.getHeight(); @@ -496,17 +448,6 @@ void vpMeNurbs::seekExtremities(const vpImage &I) /*if(end != NULL) */ delete[] end; } -/*! - Seek the extremities of the edge thanks to a canny edge detection. - The edge detection enable to find the points belonging to the edge. - The any vpMesite are initialized at this points. - - This method is useful when the edge is not smooth. - - \note To use the canny detection, OpenCV has to be installed. - - \param I : Image in which the edge appears. -*/ void vpMeNurbs::seekExtremitiesCanny(const vpImage &I) { vpMeSite pt = list.front(); @@ -769,16 +710,6 @@ void vpMeNurbs::seekExtremitiesCanny(const vpImage &I) } } -/*! - Resample the edge if the number of sample is less than 70% of the - expected value. - - \note The expected value is computed thanks to the length of the - nurbs and the parameter which indicates the number of pixel between - two points (vpMe::sample_step). - - \param I : Image in which the edge appears. -*/ void vpMeNurbs::reSample(const vpImage &I) { unsigned int n = numberOfSignal(); @@ -790,13 +721,6 @@ void vpMeNurbs::reSample(const vpImage &I) } } -/*! - Resample a part of the edge if two vpMeSite are too far from eachother. - In this case the method try to initialize any vpMeSite between the two - points. - - \param I : Image in which the edge appears. -*/ void vpMeNurbs::localReSample(const vpImage &I) { int rows = (int)I.getHeight(); @@ -882,11 +806,6 @@ void vpMeNurbs::localReSample(const vpImage &I) me->setRange(range_tmp); } -/*! - Suppress vpMeSites if they are too close to each other. - - The goal is to keep the order of the vpMeSites in the list. -*/ void vpMeNurbs::supressNearPoints() { #if 0 @@ -932,11 +851,6 @@ void vpMeNurbs::supressNearPoints() } } -/*! - Track the edge in the image I. - - \param I : Image in which the edge appears. -*/ void vpMeNurbs::track(const vpImage &I) { // Tracking des vpMeSites @@ -983,17 +897,6 @@ void vpMeNurbs::track(const vpImage &I) reSample(I); } -/*! - Display edge. - - \warning To effectively display the edge a call to - vpDisplay::flush() is needed. - - \param I : Image in which the edge appears. - \param color : Color of the displayed line. - \param thickness : Drawings thickness. - - */ void vpMeNurbs::display(const vpImage &I, const vpColor &color, unsigned int thickness) { vpMeNurbs::display(I, nurbs, color, thickness); @@ -1213,18 +1116,6 @@ bool vpMeNurbs::farFromImageEdge(const vpImage &I, const vpImageP return (iP.get_i() < height - 20 && iP.get_j() < width - 20 && iP.get_i() > 20 && iP.get_j() > 20); } -/*! - - Display of a moving nurbs. - - \param I : The image used as background. - - \param n : Nurbs to display - - \param color : Color used to display the nurbs. - - \param thickness : Drawings thickness. -*/ void vpMeNurbs::display(const vpImage &I, vpNurbs &n, const vpColor &color, unsigned int thickness) { double u = 0.0; @@ -1236,18 +1127,6 @@ void vpMeNurbs::display(const vpImage &I, vpNurbs &n, const vpCol } } -/*! - - Display of a moving nurbs. - - \param I : The image used as background. - - \param n : Nurbs to display - - \param color : Color used to display the nurbs. - - \param thickness : Drawings thickness. -*/ void vpMeNurbs::display(const vpImage &I, vpNurbs &n, const vpColor &color, unsigned int thickness) { double u = 0.0; diff --git a/modules/tracker/me/src/moving-edges/vpMeSite.cpp b/modules/tracker/me/src/moving-edges/vpMeSite.cpp index aec806f7f5..5e4103bb0b 100644 --- a/modules/tracker/me/src/moving-edges/vpMeSite.cpp +++ b/modules/tracker/me/src/moving-edges/vpMeSite.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: * Moving edges. - * -*****************************************************************************/ + */ /*! \file vpMeSite.cpp @@ -70,7 +68,6 @@ void vpMeSite::init() // Pixel components i = 0; j = 0; - v = 0; ifloat = i; jfloat = j; @@ -86,7 +83,7 @@ void vpMeSite::init() } vpMeSite::vpMeSite() - : i(0), j(0), ifloat(0), jfloat(0), v(0), mask_sign(1), alpha(0.), convlt(0.), normGradient(0), + : i(0), j(0), ifloat(0), jfloat(0), mask_sign(1), alpha(0.), convlt(0.), normGradient(0), weight(1), selectDisplay(NONE), state(NO_SUPPRESSION) #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS , @@ -95,7 +92,7 @@ vpMeSite::vpMeSite() { } vpMeSite::vpMeSite(double ip, double jp) - : i(0), j(0), ifloat(0), jfloat(0), v(0), mask_sign(1), alpha(0.), convlt(0.), normGradient(0), + : i(0), j(0), ifloat(0), jfloat(0), mask_sign(1), alpha(0.), convlt(0.), normGradient(0), weight(1), selectDisplay(NONE), state(NO_SUPPRESSION) #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS , @@ -108,11 +105,8 @@ vpMeSite::vpMeSite(double ip, double jp) jfloat = jp; } -/*! - Copy constructor. -*/ vpMeSite::vpMeSite(const vpMeSite &mesite) - : i(0), j(0), ifloat(0), jfloat(0), v(0), mask_sign(1), alpha(0.), convlt(0.), normGradient(0), + : i(0), j(0), ifloat(0), jfloat(0), mask_sign(1), alpha(0.), convlt(0.), normGradient(0), weight(1), selectDisplay(NONE), state(NO_SUPPRESSION) #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS , @@ -135,11 +129,9 @@ void vpMeSite::init(double ip, double jp, double alphap) j = vpMath::round(jp); alpha = alphap; mask_sign = 1; - - v = 0; } -// initialise with convolution +// initialise with convolution() void vpMeSite::init(double ip, double jp, double alphap, double convltp) { selectDisplay = NONE; @@ -150,8 +142,6 @@ void vpMeSite::init(double ip, double jp, double alphap, double convltp) alpha = alphap; convlt = convltp; mask_sign = 1; - - v = 0; } // initialise with convolution and sign @@ -165,8 +155,6 @@ void vpMeSite::init(double ip, double jp, double alphap, double convltp, int sig alpha = alphap; convlt = convltp; mask_sign = sign; - - v = 0; } vpMeSite &vpMeSite::operator=(const vpMeSite &m) @@ -175,7 +163,6 @@ vpMeSite &vpMeSite::operator=(const vpMeSite &m) j = m.j; ifloat = m.ifloat; jfloat = m.jfloat; - v = m.v; mask_sign = m.mask_sign; alpha = m.alpha; convlt = m.convlt; @@ -191,14 +178,6 @@ vpMeSite &vpMeSite::operator=(const vpMeSite &m) return *this; } -/*! - * Construct and return the list of vpMeSite along the normal to the contour, - * in the given range. - * \pre : ifloat, jfloat, and the direction of the normal (alpha) have to be set. - * \param I : Image in which the display is performed. - * \param range : +/- the range within which the pixel's correspondent will be sought. - * \return Pointer to the list of query sites - */ vpMeSite *vpMeSite::getQueryList(const vpImage &I, const int range) { unsigned int range_ = static_cast(range); @@ -263,7 +242,6 @@ void vpMeSite::getSign(const vpImage &I, const int range) unsigned int i2 = static_cast(vpMath::round(ifloat + k * salpha)); unsigned int j2 = static_cast(vpMath::round(jfloat + k * calpha)); - // TODO: Here check if i1,j1,i2,j2 > 0 else ?? if (I[i1][j1] > I[i2][j2]) mask_sign = 1; else @@ -324,13 +302,6 @@ double vpMeSite::convolution(const vpImage &I, const vpMe *me) return (conv); } -/*! - - Specific function for moving-edges. - - \warning To display the moving edges graphics a call to vpDisplay::flush() is needed after this function. - -*/ void vpMeSite::track(const vpImage &I, const vpMe *me, bool test_contrast) { int max_rank = -1; @@ -447,21 +418,6 @@ void vpMeSite::display(const vpImage &I) { vpMeSite::display(I, ifloat, // Static functions -/*! - Display the moving edge site with a color corresponding to their state. - - - If green : The vpMeSite is a good point. - - If blue : The point is removed because of the vpMeSite tracking phase (contrast problem). - - If purple : The point is removed because of the vpMeSite tracking phase (threshold problem). - - If red : The point is removed because of the robust method in the virtual visual servoing (M-Estimator problem). - - If cyan : The point is removed because it's too close to another. - - Yellow otherwise. - - \param I : The image. - \param i : Pixel i of the site. - \param j : Pixel j of the site. - \param state : State of the site. -*/ void vpMeSite::display(const vpImage &I, const double &i, const double &j, const vpMeSiteState &state) { switch (state) { @@ -490,21 +446,6 @@ void vpMeSite::display(const vpImage &I, const double &i, const d } } -/*! - Display the moving edge site with a color corresponding to their state. - - - If green : The vpMeSite is a good point. - - If blue : The point is removed because of the vpMeSite tracking phase (contrast problem). - - If purple : The point is removed because of the vpMeSite tracking phase (threshold problem). - - If red : The point is removed because of the robust method in the virtual visual servoing (M-Estimator problem). - - If cyan : The point is removed because it's too close to another. - - Yellow otherwise - - \param I : The image. - \param i : Pixel i of the site. - \param j : Pixel j of the site. - \param state : State of the site. -*/ void vpMeSite::display(const vpImage &I, const double &i, const double &j, const vpMeSiteState &state) { switch (state) { diff --git a/modules/tracker/me/src/moving-edges/vpMeTracker.cpp b/modules/tracker/me/src/moving-edges/vpMeTracker.cpp index 32e4dcebaa..7ff2234fe0 100644 --- a/modules/tracker/me/src/moving-edges/vpMeTracker.cpp +++ b/modules/tracker/me/src/moving-edges/vpMeTracker.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2019 by Inria. All rights reserved. * @@ -30,12 +29,11 @@ * * Description: * Moving edges. - * -*****************************************************************************/ + */ /*! -\file vpMeTracker.cpp -\brief Contains abstract elements for a Distance to Feature type feature. + \file vpMeTracker.cpp + \brief Contains abstract elements for a Distance to Feature type feature. */ #include @@ -87,9 +85,6 @@ vpMeTracker::vpMeTracker(const vpMeTracker &meTracker) #endif } -/*! - Reset the tracker by removing all the moving edges. - */ void vpMeTracker::reset() { nGoodElement = 0; @@ -125,15 +120,6 @@ unsigned int vpMeTracker::numberOfSignal() unsigned int vpMeTracker::totalNumberOfSignal() { return (unsigned int)list.size(); } -/*! - Test whether the pixel is inside the mask. Mask values that are set to true - are considered in the tracking. - - \param mask: Mask image or NULL if not wanted. Mask values that are set to true - are considered in the tracking. To disable a pixel, set false. - \param i : Pixel coordinate along the rows. - \param j : Pixel coordinate along the columns. -*/ bool vpMeTracker::inMask(const vpImage *mask, unsigned int i, unsigned int j) { try { @@ -156,13 +142,6 @@ int vpMeTracker::outOfImage(const vpImagePoint &iP, int half, int rows, int cols return (!((i > half + 2) && (i < rows - (half + 2)) && (j > half + 2) && (j < cols - (half + 2)))); } -/*! - Virtual function that is called by lower classes vpMeEllipse, vpMeLine - and vpMeNurbs. - - \exception vpTrackingException::initializationError : Moving edges not - initialized. -*/ void vpMeTracker::initTracking(const vpImage &I) { if (!me) { @@ -200,14 +179,6 @@ void vpMeTracker::initTracking(const vpImage &I) me->setRange(range_tmp); } -/*! - Track moving-edges. - - \param I : Image. - - \exception vpTrackingException::initializationError : Moving edges not initialized. - -*/ void vpMeTracker::track(const vpImage &I) { if (!me) { @@ -255,18 +226,6 @@ void vpMeTracker::track(const vpImage &I) } } -/*! - Display the moving edge sites with a color corresponding to their state. - - - If green : The vpMeSite is a good point. - - If blue : The point is removed because of the vpMeSite tracking phase (contrast problem). - - If purple : The point is removed because of the vpMeSite tracking phase (threshold problem). - - If red : The point is removed because of the robust method in the virtual visual servoing (M-Estimator problem). - - If cyan : The point is removed because it's too close to another. - - Yellow otherwise - - \param I : The image. -*/ void vpMeTracker::display(const vpImage &I) { for (std::list::const_iterator it = list.begin(); it != list.end(); ++it) { @@ -283,12 +242,6 @@ void vpMeTracker::display(const vpImage &I) } } -/*! Displays the status of moving edge sites - - \param I : The image. - \param w : vector - \param index_w : index -*/ void vpMeTracker::display(const vpImage &I, vpColVector &w, unsigned int &index_w) { for (std::list::iterator it = list.begin(); it != list.end(); ++it) { diff --git a/modules/tracker/me/src/moving-edges/vpNurbs.cpp b/modules/tracker/me/src/moving-edges/vpNurbs.cpp index f05c019873..ec9b0ec379 100644 --- a/modules/tracker/me/src/moving-edges/vpNurbs.cpp +++ b/modules/tracker/me/src/moving-edges/vpNurbs.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: * This class implements the Non Uniform Rational B-Spline (NURBS) - * -*****************************************************************************/ + */ #include // std::fabs #include // numeric_limits @@ -48,37 +46,12 @@ inline double distance(const vpImagePoint &iP1, double w1, const vpImagePoint &i return sqrt(vpMath::sqr(distancei) + vpMath::sqr(distancej) + vpMath::sqr(distancew)); } -/*! - Basic constructor. - - The degree \f$ p \f$ of the NURBS basis functions is set to 3 to - compute cubic NURBS. -*/ vpNurbs::vpNurbs() : weights() { p = 3; } -/*! - Copy constructor. -*/ -vpNurbs::vpNurbs(const vpNurbs &nurbs) : vpBSpline(nurbs), weights(nurbs.weights) {} +vpNurbs::vpNurbs(const vpNurbs &nurbs) : vpBSpline(nurbs), weights(nurbs.weights) { } -/*! - Basic destructor -*/ -vpNurbs::~vpNurbs() {} +vpNurbs::~vpNurbs() { } -/*! - Compute the coordinates of a point \f$ C(u) = \frac{\sum_{i=0}^n - (N_{i,p}(u)w_iP_i)}{\sum_{i=0}^n (N_{i,p}(u)w_i)} \f$ corresponding to the - knot \f$ u \f$. - - \param l_u : A real number which is between the extrimities of the knot - vector \param l_i : the number of the knot interval in which \f$ l_u \f$ - lies \param l_p : Degree of the NURBS basis functions. \param l_knots : The - knot vector \param l_controlPoints : the list of control points. \param - l_weights : the list of weights. - - return the coordinates of a point corresponding to the knot \f$ u \f$. -*/ vpImagePoint vpNurbs::computeCurvePoint(double l_u, unsigned int l_i, unsigned int l_p, std::vector &l_knots, std::vector &l_controlPoints, std::vector &l_weights) { @@ -104,15 +77,6 @@ vpImagePoint vpNurbs::computeCurvePoint(double l_u, unsigned int l_i, unsigned i return pt; } -/*! - Compute the coordinates of a point \f$ C(u) = \frac{\sum_{i=0}^n - (N_{i,p}(u)w_iP_i)}{\sum_{i=0}^n (N_{i,p}(u)w_i)} \f$ corresponding to the - knot \f$ u \f$. - - \param u : A real number which is between the extrimities of the knot vector - - return the coordinates of a point corresponding to the knot \f$ u \f$. -*/ vpImagePoint vpNurbs::computeCurvePoint(double u) { vpBasisFunction *N = NULL; @@ -137,33 +101,6 @@ vpImagePoint vpNurbs::computeCurvePoint(double u) return pt; } -/*! - This function is used in the computeCurveDersPoint method. - - Compute the kth derivatives of \f$ C(u) \f$ for \f$ k = 0, ... , l_{der} - \f$. - - The formula used is the following : - - \f[ C^{(k)}(u) = \sum_{i=0}^n (N_{i,p}^{(k)}(u)Pw_i) \f] - - where \f$ i \f$ is the knot interval number in which \f$ u \f$ lies, \f$ p - \f$ is the degree of the NURBS basis function and \f$ Pw_i = (P_i w_i) \f$ - contains the control points and the associatede weights. - - \param l_u : A real number which is between the extrimities of the knot - vector \param l_i : the number of the knot interval in which \f$ l_u \f$ - lies \param l_p : Degree of the NURBS basis functions. \param l_der : The - last derivative to be computed. \param l_knots : The knot vector \param - l_controlPoints : the list of control points. \param l_weights : the list of - weights. - - \return a matrix of size (l_der+1)x3 containing the coordinates \f$ - C^{(k)}(u) \f$ for \f$ k = 0, ... , l_{der} \f$. The kth derivative is in - the kth line of the matrix. For each lines the first and the second column - coresponds to the coordinates (i,j) of the point and the third column - corresponds to the associated weight. -*/ vpMatrix vpNurbs::computeCurveDers(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, std::vector &l_knots, std::vector &l_controlPoints, std::vector &l_weights) @@ -192,28 +129,6 @@ vpMatrix vpNurbs::computeCurveDers(double l_u, unsigned int l_i, unsigned int l_ return derivate; } -/*! - This function is used in the computeCurveDersPoint method. - - Compute the kth derivatives of \f$ C(u) \f$ for \f$ k = 0, ... , der \f$. - - The formula used is the following : - - \f[ C^{(k)}(u) = \sum_{i=0}^n (N_{i,p}^{(k)}(u)Pw_i) \f] - - where \f$ i \f$ is the knot interval number in which \f$ u \f$ lies, \f$ p - \f$ is the degree of the NURBS basis function and \f$ Pw_i = (P_i w_i) \f$ - contains the control points and the associatede weights. - - \param u : A real number which is between the extrimities of the knot vector - \param der : The last derivative to be computed. - - \return a matrix of size (l_der+1)x3 containing the coordinates \f$ - C^{(k)}(u) \f$ for \f$ k = 0, ... , der \f$. The kth derivative is in the - kth line of the matrix. For each lines the first and the second column - coresponds to the coordinates (i,j) of the point and the third column - corresponds to the associated weight. -*/ vpMatrix vpNurbs::computeCurveDers(double u, unsigned int der) { vpMatrix derivate(der + 1, 3); @@ -237,23 +152,6 @@ vpMatrix vpNurbs::computeCurveDers(double u, unsigned int der) return derivate; } -/*! - Compute the kth derivatives of \f$ C(u) \f$ for \f$ k = 0, ... , l_{der} - \f$. - - To see how the derivatives are computed refers to the Nurbs book. - - \param l_u : A real number which is between the extrimities of the knot - vector \param l_i : the number of the knot interval in which \f$ l_u \f$ - lies \param l_p : Degree of the NURBS basis functions. \param l_der : The - last derivative to be computed. \param l_knots : The knot vector \param - l_controlPoints : the list of control points. \param l_weights : the list of - weights. - - \return an array of size l_der+1 containing the coordinates \f$ C^{(k)}(u) - \f$ for \f$ k = 0, ... , l_{der} \f$. The kth derivative is in the kth cell - of the array. -*/ vpImagePoint *vpNurbs::computeCurveDersPoint(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, std::vector &l_knots, std::vector &l_controlPoints, std::vector &l_weights) @@ -284,41 +182,14 @@ vpImagePoint *vpNurbs::computeCurveDersPoint(double l_u, unsigned int l_i, unsig return CK; } -/*! - Compute the kth derivatives of \f$ C(u) \f$ for \f$ k = 0, ... , l_{der} - \f$. - To see how the derivatives are computed refers to the Nurbs book. - - \param u : A real number which is between the extrimities of the knot vector - \param der : The last derivative to be computed. - - \return an array of size l_der+1 containing the coordinates \f$ C^{(k)}(u) - \f$ for \f$ k = 0, ... , der \f$. The kth derivative is in the kth cell of - the array. -*/ vpImagePoint *vpNurbs::computeCurveDersPoint(double u, unsigned int der) { unsigned int i = findSpan(u); return computeCurveDersPoint(u, i, p, der, knots, controlPoints, weights); } -/*! - Insert \f$ l_r \f$ times a knot in the \f$ l_k \f$ th interval of the knot - vector. The inserted knot \f$ l_u \f$ has multiplicity \f$ l_s \f$. - - Of course the knot vector changes. But The list of control points and the - list of the associated weights change too. - - \param l_u : A real number which is between the extrimities of the knot - vector and which has to be inserted. \param l_k : The number of the knot - interval in which \f$ l_u \f$ lies. \param l_s : Multiplicity of \f$ l_u \f$ - \param l_r : Number of times \f$ l_u \f$ has to be inserted. - \param l_p : Degree of the NURBS basis functions. - \param l_knots : The knot vector - \param l_controlPoints : the list of control points. - \param l_weights : the list of weights. -*/ + void vpNurbs::curveKnotIns(double l_u, unsigned int l_k, unsigned int l_s, unsigned int l_r, unsigned int l_p, std::vector &l_knots, std::vector &l_controlPoints, std::vector &l_weights) @@ -371,36 +242,13 @@ void vpNurbs::curveKnotIns(double l_u, unsigned int l_k, unsigned int l_s, unsig l_knots.insert(it2 + (int)l_k, l_r, l_u); } -/*! - Insert \f$ r \f$ times a knot in the \f$ k \f$ th interval of the knot - vector. The inserted knot \f$ u \f$ has multiplicity \f$ s \f$. - - Of course the knot vector changes. But The list of control points and the - list of the associated weights change too. - - \param u : A real number which is between the extrimities of the knot vector - and which has to be inserted. \param s : Multiplicity of \f$ l_u \f$ \param - r : Number of times \f$ l_u \f$ has to be inserted. -*/ void vpNurbs::curveKnotIns(double u, unsigned int s, unsigned int r) { unsigned int i = findSpan(u); curveKnotIns(u, i, s, r, p, knots, controlPoints, weights); } -/*! - Insert \f$ l_r \f$ knots in the knot vector. - - Of course the knot vector changes. But The list of control points and the - list of the associated weights change too. - \param l_x : Several real numbers which are between the extrimities of the - knot vector and which have to be inserted. \param l_r : Number of knot in - the array \f$ l_x \f$. \param l_p : Degree of the NURBS basis functions. - \param l_knots : The knot vector - \param l_controlPoints : the list of control points. - \param l_weights : the list of weights. -*/ void vpNurbs::refineKnotVectCurve(double *l_x, unsigned int l_r, unsigned int l_p, std::vector &l_knots, std::vector &l_controlPoints, std::vector &l_weights) { @@ -461,7 +309,8 @@ void vpNurbs::refineKnotVectCurve(double *l_x, unsigned int l_r, unsigned int l_ if (std::fabs(alpha) <= std::numeric_limits::epsilon()) { l_controlPoints[ind - 1] = l_controlPoints[ind]; l_weights[ind - 1] = l_weights[ind]; - } else { + } + else { alpha = alpha / (l_knots[k + l] - l_knots_tmp[i - l_p + l]); l_controlPoints[ind - 1].set_i(alpha * l_controlPoints[ind - 1].get_i() + (1.0 - alpha) * l_controlPoints[ind].get_i()); @@ -480,46 +329,11 @@ void vpNurbs::refineKnotVectCurve(double *l_x, unsigned int l_r, unsigned int l_ } } -/*! - Insert \f$ r \f$ knots in the knot vector. - - Of course the knot vector changes. But The list of control points and the - list of the associated weights change too. - - \param x : Several real numbers which are between the extrimities of the - knot vector and which have to be inserted. \param r : Number of knot in the - array \f$ l_x \f$. -*/ void vpNurbs::refineKnotVectCurve(double *x, unsigned int r) { refineKnotVectCurve(x, r, p, knots, controlPoints, weights); } -/*! - Remove \f$ l_num \f$ times the knot \f$ l_u \f$ from the knot vector. The - removed knot \f$ l_u \f$ is the \f$ l_r \f$ th vector in the knot vector. - - Of course the knot vector changes. But The list of control points and the - list of the associated weights change too. - - \param l_u : A real number which is between the extrimities of the knot - vector and which has to be removed. \param l_r : Index of \f$ l_u \f$ in the - knot vector. \param l_num : Number of times \f$ l_u \f$ has to be removed. - \param l_TOL : A parameter which has to be computed. - \param l_s : Multiplicity of \f$ l_u \f$. - \param l_p : Degree of the NURBS basis functions. - \param l_knots : The knot vector - \param l_controlPoints : the list of control points. - \param l_weights : the list of weights. - - \return The number of time that l_u was removed. - - \f$ l_{TOL} = \frac{dw_{min}}{1+|P|_{max}} \f$ - - where \f$ w_{min} \f$ is the minimal weight on the original curve, \f$ - |P|_{max} \f$ is the maximum distance of any point on the original curve - from the origin and \f$ d \f$ is the desired bound on deviation. -*/ unsigned int vpNurbs::removeCurveKnot(double l_u, unsigned int l_r, unsigned int l_num, double l_TOL, unsigned int l_s, unsigned int l_p, std::vector &l_knots, std::vector &l_controlPoints, std::vector &l_weights) @@ -578,12 +392,13 @@ unsigned int vpNurbs::removeCurveKnot(double l_u, unsigned int l_r, unsigned int double distance = sqrt(vpMath::sqr(distancei) + vpMath::sqr(distancej) + vpMath::sqr(distancew)); if (distance <= l_TOL) remflag = 1; - } else { + } + else { alfi = (l_u - l_knots[i]) / (l_knots[i + ord + t] - l_knots[i]); double distancei = - l_controlPoints[i].get_i() - (alfi * tempP[ii + t + 1].get_i() + (1.0 - alfi) * tempP[ii - 1].get_i()); + l_controlPoints[i].get_i() - (alfi * tempP[ii + t + 1].get_i() + (1.0 - alfi) * tempP[ii - 1].get_i()); double distancej = - l_controlPoints[i].get_j() - (alfi * tempP[ii + t + 1].get_j() + (1.0 - alfi) * tempP[ii - 1].get_j()); + l_controlPoints[i].get_j() - (alfi * tempP[ii + t + 1].get_j() + (1.0 - alfi) * tempP[ii - 1].get_j()); double distancew = l_weights[i] - (alfi * tempW[ii + t + 1] + (1.0 - alfi) * tempW[ii - 1]); double distance = sqrt(vpMath::sqr(distancei) + vpMath::sqr(distancej) + vpMath::sqr(distancew)); if (distance <= l_TOL) @@ -642,43 +457,11 @@ unsigned int vpNurbs::removeCurveKnot(double l_u, unsigned int l_r, unsigned int return t; } -/*! - Remove \f$ num \f$ times the knot \f$ u \f$ from the knot vector. The - removed knot \f$ u \f$ is the \f$ r \f$ th vector in the knot vector. - - Of course the knot vector changes. But The list of control points and the - list of the associated weights change too. - - \param u : A real number which is between the extrimities of the knot vector - and which has to be removed. \param r : Index of \f$ l_u \f$ in the knot - vector. \param num : Number of times \f$ l_u \f$ has to be removed. \param - TOL : A parameter which has to be computed. - - \return The number of time that l_u was removed. - - \f$ TOL = \frac{dw_{min}}{1+|P|_{max}} \f$ - - where \f$ w_{min} \f$ is the minimal weight on the original curve, \f$ - |P|_{max} \f$ is the maximum distance of any point on the original curve - from the origin and \f$ d \f$ is the desired bound on deviation. -*/ -unsigned int vpNurbs::removeCurveKnot(double u, unsigned int r, unsigned int num, double TOL) +unsigned int vpNurbs::removeCurveKnot(double l_u, unsigned int l_r, unsigned int l_num, double l_TOL) { - return removeCurveKnot(u, r, num, TOL, 0, p, knots, controlPoints, weights); + return removeCurveKnot(l_u, l_r, l_num, l_TOL, 0, p, knots, controlPoints, weights); } -/*! - Method which enables to compute a NURBS curve passing through a set of data - points. - - The result of the method is composed by a knot vector, a set of control - points and a set of associated weights. - - \param l_crossingPoints : The list of data points which have to be - interpolated. \param l_p : Degree of the NURBS basis functions. This value - need to be > 0. \param l_knots : The knot vector \param l_controlPoints : - the list of control points. \param l_weights : the list of weights. -*/ void vpNurbs::globalCurveInterp(std::vector &l_crossingPoints, unsigned int l_p, std::vector &l_knots, std::vector &l_controlPoints, std::vector &l_weights) @@ -756,16 +539,6 @@ void vpNurbs::globalCurveInterp(std::vector &l_crossingPoints, uns } } -/*! - Method which enables to compute a NURBS curve passing through a set of data - points. - - The result of the method is composed by a knot vector, a set of control - points and a set of associated weights. - - \param l_crossingPoints : The list of data points which have to be - interpolated. -*/ void vpNurbs::globalCurveInterp(vpList &l_crossingPoints) { std::vector v_crossingPoints; @@ -787,16 +560,6 @@ void vpNurbs::globalCurveInterp(vpList &l_crossingPoints) globalCurveInterp(v_crossingPoints, p, knots, controlPoints, weights); } -/*! - Method which enables to compute a NURBS curve passing through a set of data - points. - - The result of the method is composed by a knot vector, a set of control - points and a set of associated weights. - - \param l_crossingPoints : The list of data points which have to be - interpolated. -*/ void vpNurbs::globalCurveInterp(const std::list &l_crossingPoints) { std::vector v_crossingPoints; @@ -806,16 +569,7 @@ void vpNurbs::globalCurveInterp(const std::list &l_crossingPoints) globalCurveInterp(v_crossingPoints, p, knots, controlPoints, weights); } -/*! - Method which enables to compute a NURBS curve passing through a set of data - points. - - The result of the method is composed by a knot vector, a set of control - points and a set of associated weights. - \param l_crossingPoints : The list of data points which have to be - interpolated. -*/ void vpNurbs::globalCurveInterp(const std::list &l_crossingPoints) { std::vector v_crossingPoints; @@ -835,31 +589,8 @@ void vpNurbs::globalCurveInterp(const std::list &l_crossingPoints) globalCurveInterp(v_crossingPoints, p, knots, controlPoints, weights); } -/*! - Method which enables to compute a NURBS curve passing through a set of data - points. - - The result of the method is composed by a knot vector, a set of control - points and a set of associated weights. -*/ void vpNurbs::globalCurveInterp() { globalCurveInterp(crossingPoints, p, knots, controlPoints, weights); } -/*! - Method which enables to compute a NURBS curve approximating a set of data - points. - - The data points are approximated thanks to a least square method. - - The result of the method is composed by a knot vector, a set of control - points and a set of associated weights. - - \param l_crossingPoints : The list of data points which have to be - interpolated. \param l_p : Degree of the NURBS basis functions. \param l_n : - The desired number of control points. l_n must be under or equal to the - number of data points. \param l_knots : The knot vector. \param - l_controlPoints : the list of control points. \param l_weights : the list of - weights. -*/ void vpNurbs::globalCurveApprox(std::vector &l_crossingPoints, unsigned int l_p, unsigned int l_n, std::vector &l_knots, std::vector &l_controlPoints, std::vector &l_weights) @@ -908,19 +639,22 @@ void vpNurbs::globalCurveApprox(std::vector &l_crossingPoints, uns N[l_p].value * l_crossingPoints[m].get_j()); Rk.push_back(pt); delete[] N; - } else if (span == l_p) { + } + else if (span == l_p) { N = computeBasisFuns(ubar[k], span, l_p, l_knots); vpImagePoint pt(l_crossingPoints[k].get_i() - N[0].value * l_crossingPoints[0].get_i(), l_crossingPoints[k].get_j() - N[0].value * l_crossingPoints[0].get_j()); Rk.push_back(pt); delete[] N; - } else if (span == l_n) { + } + else if (span == l_n) { N = computeBasisFuns(ubar[k], span, l_p, l_knots); vpImagePoint pt(l_crossingPoints[k].get_i() - N[l_p].value * l_crossingPoints[m].get_i(), l_crossingPoints[k].get_j() - N[l_p].value * l_crossingPoints[m].get_j()); Rk.push_back(pt); delete[] N; - } else { + } + else { Rk.push_back(l_crossingPoints[k]); } } @@ -975,22 +709,7 @@ void vpNurbs::globalCurveApprox(std::vector &l_crossingPoints, uns l_weights.push_back(1.0); } -/*! - - Method which enables to compute a NURBS curve approximating a set of - data points. - - The data points are approximated thanks to a least square method. - The result of the method is composed by a knot vector, a set of - control points and a set of associated weights. - - \param l_crossingPoints : The list of data points which have to be - interpolated. - - \param n : The desired number of control points. This parameter \e n - must be under or equal to the number of data points. -*/ void vpNurbs::globalCurveApprox(vpList &l_crossingPoints, unsigned int n) { std::vector v_crossingPoints; @@ -1004,19 +723,7 @@ void vpNurbs::globalCurveApprox(vpList &l_crossingPoints, unsigned int globalCurveApprox(v_crossingPoints, p, n, knots, controlPoints, weights); } -/*! - Method which enables to compute a NURBS curve approximating a set of data - points. - The data points are approximated thanks to a least square method. - - The result of the method is composed by a knot vector, a set of control - points and a set of associated weights. - - \param l_crossingPoints : The list of data points which have to be - interpolated. \param n : The desired number of control points. The parameter - \e n must be under or equal to the number of data points. -*/ void vpNurbs::globalCurveApprox(const std::list &l_crossingPoints, unsigned int n) { std::vector v_crossingPoints; @@ -1026,22 +733,6 @@ void vpNurbs::globalCurveApprox(const std::list &l_crossingPoints, globalCurveApprox(v_crossingPoints, p, n, knots, controlPoints, weights); } -/*! - - Method which enables to compute a NURBS curve approximating a set of - data points. - - The data points are approximated thanks to a least square method. - - The result of the method is composed by a knot vector, a set of - control points and a set of associated weights. - - \param l_crossingPoints : The list of data points which have to be - interpolated. - - \param n : The desired number of control points. This parameter \e n - must be under or equal to the number of data points. -*/ void vpNurbs::globalCurveApprox(const std::list &l_crossingPoints, unsigned int n) { std::vector v_crossingPoints; @@ -1052,15 +743,6 @@ void vpNurbs::globalCurveApprox(const std::list &l_crossingPoints, uns globalCurveApprox(v_crossingPoints, p, n, knots, controlPoints, weights); } -/*! - Method which enables to compute a NURBS curve approximating a set of data - points. - - The data points are approximated thanks to a least square method. - - The result of the method is composed by a knot vector, a set of control - points and a set of associated weights. -*/ void vpNurbs::globalCurveApprox(unsigned int n) { globalCurveApprox(crossingPoints, p, n, knots, controlPoints, weights); diff --git a/modules/tracker/me/test/testJsonMe.cpp b/modules/tracker/me/test/testJsonMe.cpp index 185cdd6401..1874ae1a95 100644 --- a/modules/tracker/me/test/testJsonMe.cpp +++ b/modules/tracker/me/test/testJsonMe.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 vpCameraParameters JSON parse / save. - * -*****************************************************************************/ + */ /*! \file testJsonMe.cpp @@ -48,7 +46,7 @@ #include #include -using json = nlohmann::json; +using json = nlohmann::json; //! json namespace shortcut #define CATCH_CONFIG_RUNNER #include diff --git a/modules/tracker/me/test/testNurbs.cpp b/modules/tracker/me/test/testNurbs.cpp index 0a5f33d295..97dc6af1b6 100644 --- a/modules/tracker/me/test/testNurbs.cpp +++ b/modules/tracker/me/test/testNurbs.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: * Exemple of a Nurbs curve. - * -*****************************************************************************/ + */ /*! \example testNurbs.cpp diff --git a/modules/vision/include/visp3/vision/vpBasicKeyPoint.h b/modules/vision/include/visp3/vision/vpBasicKeyPoint.h index d1442c319b..badad5522d 100644 --- a/modules/vision/include/visp3/vision/vpBasicKeyPoint.h +++ b/modules/vision/include/visp3/vision/vpBasicKeyPoint.h @@ -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: * Key point used in matching algorithm. - * -*****************************************************************************/ + */ #ifndef vpBasicKeyPoint_H #define vpBasicKeyPoint_H @@ -59,8 +57,16 @@ class VISP_EXPORT vpBasicKeyPoint { public: - vpBasicKeyPoint(); + /*! + * Basic constructor. + */ + vpBasicKeyPoint() + : referenceImagePointsList(), currentImagePointsList(), matchedReferencePoints(), _reference_computed(false) + { } + /*! + * Destructor. + */ virtual ~vpBasicKeyPoint() { matchedReferencePoints.resize(0); @@ -68,50 +74,74 @@ class VISP_EXPORT vpBasicKeyPoint referenceImagePointsList.resize(0); } + /*! + * Build reference. + */ virtual unsigned int buildReference(const vpImage &I) = 0; + /*! + * Build reference. + */ virtual unsigned int buildReference(const vpImage &I, const vpImagePoint &iP, unsigned int height, - unsigned int width) = 0; + unsigned int width) = 0; + /*! + * Build reference. + */ virtual unsigned int buildReference(const vpImage &I, const vpRect &rectangle) = 0; + /*! + * Match keypoints. + */ virtual unsigned int matchPoint(const vpImage &I) = 0; + /*! + * Match keypoints. + */ virtual unsigned int matchPoint(const vpImage &I, const vpImagePoint &iP, unsigned int height, unsigned int width) = 0; + /*! + * Match keypoints. + */ virtual unsigned int matchPoint(const vpImage &I, const vpRect &rectangle) = 0; + /*! + * Display keypoints. + */ virtual void display(const vpImage &Iref, const vpImage &Icurrent, unsigned int size = 3) = 0; + /*! + * Display keypoints. + */ virtual void display(const vpImage &Icurrent, unsigned int size = 3, - const vpColor &color = vpColor::green) = 0; + const vpColor &color = vpColor::green) = 0; /*! - Indicate wether the reference has been built or not. - - \return True if the reference of the current instance has been built. - */ + * Indicate wether the reference has been built or not. + * + * \return True if the reference of the current instance has been built. + */ bool referenceBuilt() const { return _reference_computed; } /*! - Get the pointer to the complete list of reference points. The pointer is - const. Thus the points can not be modified - - \return The pointer to the complete list of reference points. - */ + * Get the pointer to the complete list of reference points. The pointer is + * const. Thus the points can not be modified + * + * \return The pointer to the complete list of reference points. + */ inline const vpImagePoint *getAllPointsInReferenceImage() { return &referenceImagePointsList[0]; } /*! - Get the nth reference point. This point is copied in the vpImagePoint - instance given in argument. - - \param index : The index of the desired reference point. The index must be - between 0 and the number of reference points - 1. - \param referencePoint : - The coordinates of the desired reference point are copied there. - */ + * Get the nth reference point. This point is copied in the vpImagePoint + * instance given in argument. + * + * \param index : The index of the desired reference point. The index must be + * between 0 and the number of reference points - 1. + * \param referencePoint : + * The coordinates of the desired reference point are copied there. + */ inline void getReferencePoint(unsigned int index, vpImagePoint &referencePoint) { if (index >= referenceImagePointsList.size()) { @@ -123,16 +153,16 @@ class VISP_EXPORT vpBasicKeyPoint } /*! - Get the nth couple of reference point and current point which have been - matched. These points are copied in the vpImagePoint instances given in - argument. - - \param index : The index of the desired couple of reference point and - current point. The index must be between 0 and the number of matched - points - 1. - \param referencePoint : The coordinates of the desired reference point are copied here. - \param currentPoint : The coordinates of the desired current point are copied here. - */ + * Get the nth couple of reference point and current point which have been + * matched. These points are copied in the vpImagePoint instances given in + * argument. + * + * \param index : The index of the desired couple of reference point and + * current point. The index must be between 0 and the number of matched + * points - 1. + * \param referencePoint : The coordinates of the desired reference point are copied here. + * \param currentPoint : The coordinates of the desired current point are copied here. + */ inline void getMatchedPoints(unsigned int index, vpImagePoint &referencePoint, vpImagePoint ¤tPoint) { if (index >= matchedReferencePoints.size()) { @@ -145,29 +175,28 @@ class VISP_EXPORT vpBasicKeyPoint } /*! - Get the nth matched reference point index in the complete list of - reference point. - - In the code below referencePoint1 and referencePoint2 correspond to the - same matched reference point. - - \code - vpKeyPoint keypoint; - - //Here the code to compute the reference points and the current points. - - vpImagePoint referencePoint1; - vpImagePoint currentPoint; - keypoint.getMatchedPoints(1, referencePoint1, currentPoint); //Get the first matched points - - vpImagePoint referencePoint2; - const vpImagePoint* referencePointsList = keypoint.getAllPointsInReferenceImage(); - // Get the first matched reference point index in the complete reference point list - int index = keypoint.getIndexInAllReferencePointList(1); - // Get the first matched reference point - referencePoint2 = referencePointsList[index]; - \endcode - */ + * Get the nth matched reference point index in the complete list of + * reference point. + * + * In the code below referencePoint1 and referencePoint2 correspond to the + * same matched reference point. + * + * \code + * vpKeyPoint keypoint; + * + * // Here the code to compute the reference points and the current points. + * vpImagePoint referencePoint1; + * vpImagePoint currentPoint; + * keypoint.getMatchedPoints(1, referencePoint1, currentPoint); //Get the first matched points + * + * vpImagePoint referencePoint2; + * const vpImagePoint* referencePointsList = keypoint.getAllPointsInReferenceImage(); + * // Get the first matched reference point index in the complete reference point list + * int index = keypoint.getIndexInAllReferencePointList(1); + * // Get the first matched reference point + * referencePoint2 = referencePointsList[index]; + * \endcode + */ inline unsigned int getIndexInAllReferencePointList(unsigned int indexInMatchedPointList) { if (indexInMatchedPointList >= matchedReferencePoints.size()) { @@ -178,46 +207,46 @@ class VISP_EXPORT vpBasicKeyPoint } /*! - Get the number of reference points. - - \return the number of reference points. - */ + * Get the number of reference points. + * + * \return the number of reference points. + */ inline unsigned int getReferencePointNumber() const { return (unsigned int)referenceImagePointsList.size(); }; /*! - Get the number of matched points. - - \return the number of matched points. - */ + * Get the number of matched points. + * + * \return the number of matched points. + */ inline unsigned int getMatchedPointNumber() const { return (unsigned int)matchedReferencePoints.size(); }; /*! - Return the vector of reference image point. - - \warning Should not be modified. - - \return Vector of reference image point. - */ + * Return the vector of reference image point. + * + * \warning Should not be modified. + * + * \return Vector of reference image point. + */ const std::vector &getReferenceImagePointsList() const { return referenceImagePointsList; } /*! - Return the vector of current image point. - - \warning Should not be modified. - - \return Vector of the current image point. - */ + * Return the vector of current image point. + * + * \warning Should not be modified. + * + * \return Vector of the current image point. + */ const std::vector &getCurrentImagePointsList() const { return currentImagePointsList; } /*! - Return the index of the matched associated to the current image point i. - The ith element of the vector is the index of the reference image point - matching with the current image point. - - \warning Should not be modified. - - \return The vector of matching index. - */ + * Return the index of the matched associated to the current image point i. + * The ith element of the vector is the index of the reference image point + * matching with the current image point. + * + * \warning Should not be modified. + * + * \return The vector of matching index. + */ const std::vector &getMatchedReferencePoints() const { return matchedReferencePoints; } private: @@ -225,27 +254,29 @@ class VISP_EXPORT vpBasicKeyPoint protected: /*! - List of the points which define the reference. - */ + * List of the points which define the reference. + */ std::vector referenceImagePointsList; /*! - List of the points which belong to the current image and have - been matched with points belonging to the reference. - */ + * List of the points which belong to the current image and have + * been matched with points belonging to the reference. + */ std::vector currentImagePointsList; /*! - Array containing the index in the array "referenceImagePointsList" of the - reference points which have been matched. - - The first element of the "currentImagePointsList" array is matched with - the nth element of the "referenceImagePointsList" array. The value of n is - stored in the first element of the "matchedReferencePoints" array. - */ + * Array containing the index in the array "referenceImagePointsList" of the + * reference points which have been matched. + * + * The first element of the "currentImagePointsList" array is matched with + * the nth element of the "referenceImagePointsList" array. The value of n is + * stored in the first element of the "matchedReferencePoints" array. + */ std::vector matchedReferencePoints; - //! flag to indicate if the reference has been built. + /*! + * Flag to indicate if the reference has been built. + */ bool _reference_computed; }; diff --git a/modules/vision/include/visp3/vision/vpCalibration.h b/modules/vision/include/visp3/vision/vpCalibration.h index 15bdee551e..285c3f83ac 100644 --- a/modules/vision/include/visp3/vision/vpCalibration.h +++ b/modules/vision/include/visp3/vision/vpCalibration.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,12 +29,7 @@ * * Description: * Camera calibration. - * - * Authors: - * Francois Chaumette - * Anthony Saunier - * -*****************************************************************************/ + */ /*! \file vpCalibration.h @@ -60,20 +54,20 @@ #include #include /*! - \class vpCalibration - - \ingroup group_vision_calib - - \brief Tools for perspective camera calibration. - -*/ + * \class vpCalibration + * + * \ingroup group_vision_calib + * + * \brief Tools for perspective camera calibration. + */ class VISP_EXPORT vpCalibration { public: /*! - Minimization algorithm use to estimate the camera parameters. - */ - typedef enum { + * Minimization algorithm use to estimate the camera parameters. + */ + typedef enum + { CALIB_LAGRANGE, /*!< Lagrange approach without estimation of the distortion. */ CALIB_VIRTUAL_VS, /*!< Virtual visual servoing approach without estimation @@ -89,36 +83,69 @@ class VISP_EXPORT vpCalibration estimation of the distortion. */ } vpCalibrationMethodType; - vpHomogeneousMatrix cMo; //!< Pose computed using camera parameters without distortion - //!< (as a 3x4 matrix [R T]) - vpHomogeneousMatrix cMo_dist; //!< Pose computed using camera parameters with distortion - //!< with distortion model - //!< (as a 3x4 matrix [R T]) - vpCameraParameters cam; //!< Camera intrinsic parameters for perspective - //!< projection model without distortion - vpCameraParameters cam_dist; //!< Camera intrinsic parameters for perspective - //!< projection model with distortion - - vpHomogeneousMatrix rMe; //!< Position of the effector in relation to the - //!< reference coordinates (manipulator base coordinates) - vpHomogeneousMatrix eMc; //!< Position of the camera in end-effector frame using camera parameters without distortion - vpHomogeneousMatrix - eMc_dist; //!< Position of the camera in end-effector frame using camera parameters with distortion + /*! + * Pose computed using camera parameters without distortion (as a 3x4 matrix [R T]) + */ + vpHomogeneousMatrix cMo; + /*! + * Pose computed using camera parameters with distortion with distortion model + * (as a 3x4 matrix [R T]) + */ + vpHomogeneousMatrix cMo_dist; + /*! + * Camera intrinsic parameters for perspective projection model without distortion + */ + vpCameraParameters cam; + /*! + * Camera intrinsic parameters for perspective projection model with distortion + */ + vpCameraParameters cam_dist; - double m_aspect_ratio; //!< Fix aspect ratio (px/py) + /*! + * Position of the effector in relation to the reference coordinates (manipulator base coordinates) + */ + vpHomogeneousMatrix rMe; + /*! + * Position of the camera in end-effector frame using camera parameters without distortion + */ + vpHomogeneousMatrix eMc; + /*! + * Position of the camera in end-effector frame using camera parameters with distortion + */ + vpHomogeneousMatrix eMc_dist; + /*! + * Fix aspect ratio (px/py) + */ + double m_aspect_ratio; public: - // Constructor + /*! + * Default constructor. + */ vpCalibration(); + + /*! + * Copy constructor. + */ vpCalibration(const vpCalibration &c); - // Destructor + /*! + * Destructor : delete the array of point (freed the memory) + */ virtual ~vpCalibration(); - // Add a new point in this array + /*! + * Add a new point in the array of points. + * \param X,Y,Z : 3D coordinates of a point in the object frame + * \param ip : 2D Coordinates of the point in the camera frame. + */ int addPoint(double X, double Y, double Z, vpImagePoint &ip); - // = operator + /*! + * Copy operator. + * + * \param twinCalibration : object to be copied + */ vpCalibration &operator=(const vpCalibration &twinCalibration); #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) @@ -133,42 +160,169 @@ class VISP_EXPORT vpCalibration //@} #endif - //! Suppress all the point in the array of point + /*! + * Suppress all the point in the array of point. + */ int clearPoint(); - void computeStdDeviation(double &deviation, double &deviation_dist); + /*! + * Compute the calibration according to the desired method using one pose. + * + * \param method : Method that will be used to estimate the parameters. + * \param cMo_est : estimated homogeneous matrix that defines the pose. + * \param cam_est : estimated intrinsic camera parameters. + * \param verbose : set at true if information about the residual at each loop + * of the algorithm is hoped. + * + * \return EXIT_SUCCESS if the calibration succeed, EXIT_FAILURE otherwise. + */ int computeCalibration(vpCalibrationMethodType method, vpHomogeneousMatrix &cMo_est, vpCameraParameters &cam_est, bool verbose = false); + + /*! + * Compute the multi-images calibration according to the desired method using + * many poses. + * + * \param method : Method used to estimate the camera parameters. + * \param table_cal : Vector of vpCalibration. + * \param cam_est : Estimated intrinsic camera parameters. + * \param globalReprojectionError : Global reprojection error or global + * residual. + * \param verbose : Set at true if information about the residual at + * each loop of the algorithm is hoped. + * + * \return EXIT_SUCCESS if the calibration succeed, EXIT_FAILURE otherwise. + */ static int computeCalibrationMulti(vpCalibrationMethodType method, std::vector &table_cal, - vpCameraParameters &cam, double &globalReprojectionError, bool verbose = false); + vpCameraParameters &cam_est, double &globalReprojectionError, bool verbose = false); + + /*! + * Compute and return the standard deviation expressed in pixel + * for pose matrix and camera intrinsic parameters. + * \param deviation : the standard deviation computed for the model without + * distortion. + * \param deviation_dist : the standard deviation computed for the + * model with distortion. + */ + void computeStdDeviation(double &deviation, double &deviation_dist); + /*! + * Compute and return the standard deviation expressed in pixel + * for pose matrix and camera intrinsic parameters for model without + * distortion. + * + * \param cMo_est : the matrix that defines the pose to be tested. + * \param camera : camera intrinsic parameters to be tested. + * \return the standard deviation by point of the error in pixel . + */ double computeStdDeviation(const vpHomogeneousMatrix &cMo_est, const vpCameraParameters &camera); - double computeStdDeviation_dist(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam); + + /*! + * Compute and return the standard deviation expressed in pixel + * for pose matrix and camera intrinsic parameters with pixel to meter model. + * + * \param cMo_est : the matrix that defines the pose to be tested. + * \param camera : camera intrinsic parameters to be tested. + * \return the standard deviation by point of the error in pixel . + */ + double computeStdDeviation_dist(const vpHomogeneousMatrix &cMo_est, const vpCameraParameters &camera); + + /*! + * Display the data of the calibration (center of the tracked dots). + * + * \param I : Image where to display data. + * \param color : Color of the data. + * \param thickness : Thickness of the displayed data. + * \param subsampling_factor : Subsampling factor. Default value is 1. + * Admissible values are multiple of 2. Divide by this parameter the + * coordinates of the data points resulting from image processing. + */ int displayData(vpImage &I, vpColor color = vpColor::red, unsigned int thickness = 1, int subsampling_factor = 1); + + /*! + * Display estimated centers of dots using intrinsic camera parameters + * with model with distortion and the computed pose. + * \param I : Image where to display grid data. + * \param color : Color of the data. + * \param thickness : Thickness of the displayed data. + * \param subsampling_factor : Subsampling factor. Default value is 1. + * Admissible values are multiple of 2. Divide by this parameter the + * values of the camera parameters. + */ int displayGrid(vpImage &I, vpColor color = vpColor::yellow, unsigned int thickness = 1, int subsampling_factor = 1); //! Set the gain for the virtual visual servoing algorithm. static double getLambda() { return gain; } - //! get the residual in pixels + /*! + * Get the residual in pixels. + */ double getResidual(void) const { return residual; } - //! get the residual for perspective projection with distortion (in pixels) + + /*! + * Get the residual for perspective projection with distortion (in pixels). + */ double getResidual_dist(void) const { return residual_dist; } - //! get the number of points + + /*! + * Get the number of points. + */ unsigned int get_npt() const { return npt; } + /*! + * Basic initialisation (called by the constructors). + */ int init(); - int readData(const char *filename); - static int readGrid(const char *filename, unsigned int &n, std::list &oX, std::list &oY, + /*! + * Read data from disk : + * data are organized as follow oX oY oZ u v + * + * \param filename : Name of the file. + */ + int readData(const std::string &filename); + + /*! + * Read calibration grid coordinates from disk. + * Data are organized as follow oX oY oZ + * + * \param filename : Name of the file. + * \param n : Number of points in the calibration grid. + * \param oX : List of oX coordinates. + * \param oY : List of oY coordinates. + * \param oZ : List of oZ coordinates. + * \param verbose : Additional printings if true (number of points on + * the calibration grid and their respective coordinates in the object + * frame). + * + * \return 0 if success, -1 if an error occurs. + */ + static int readGrid(const std::string &filename, unsigned int &n, std::list &oX, std::list &oY, std::list &oZ, bool verbose = false); - //! set the gain for the virtual visual servoing algorithm + /*! + * Set the gain for the virtual visual servoing algorithm. + */ static void setLambda(const double &lambda) { gain = lambda; } + + /*! + * Set pixel aspect ratio px/py. + * + * \param[in] aspect_ratio : px/py aspect ratio. Value need to be positive. + * To estimate a model where px=py set 1 as aspect ratio. + */ void setAspectRatio(double aspect_ratio); - int writeData(const char *filename); + + /*! + * Write data into a file. + * + * Data are organized as follow oX oY oZ u v + * + * \param filename : Name of the file. + */ + int writeData(const std::string &filename); private: void computePose(const vpCameraParameters &cam, vpHomogeneousMatrix &cMo); @@ -191,7 +345,7 @@ class VISP_EXPORT vpCalibration private: unsigned int npt; //!< number of points used in calibration computation std::list LoX, LoY, - LoZ; //!< list of points coordinates (3D in meters) + LoZ; //!< list of points coordinates (3D in meters) std::list Lip; //!< list of points coordinates (2D in pixels) double residual; //!< residual in pixel for camera model without distortion @@ -204,9 +358,3 @@ class VISP_EXPORT vpCalibration }; #endif - -/* - * Local variables: - * c-basic-offset: 2 - * End: - */ diff --git a/modules/vision/include/visp3/vision/vpCalibrationException.h b/modules/vision/include/visp3/vision/vpCalibrationException.h index daf94b3abb..1b8471e5f9 100644 --- a/modules/vision/include/visp3/vision/vpCalibrationException.h +++ b/modules/vision/include/visp3/vision/vpCalibrationException.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,64 +29,48 @@ * * Description: * Exceptions that can be emitted by the vpCalibration class and its derivatives. - * - * Authors: - * Anthony Saunier - * -*****************************************************************************/ + */ #ifndef _vpCalibrationException_h_ #define _vpCalibrationException_h_ -/* ------------------------------------------------------------------------- - */ -/* --- INCLUDE ------------------------------------------------------------- - */ -/* ------------------------------------------------------------------------- - */ - -/* Classes standards. */ -// #include #include -#include /* Classe std::ostream. */ -#include /* Classe string. */ - -/* ------------------------------------------------------------------------- - */ -/* --- CLASS --------------------------------------------------------------- - */ -/* ------------------------------------------------------------------------- - */ +#include +#include /*! - - \class vpCalibrationException - \brief Error that can be emitted by the vpCalibration class. + * \class vpCalibrationException + * \brief Error that can be emitted by the vpCalibration class. */ class VISP_EXPORT vpCalibrationException : public vpException { public: /*! - \brief Lists the possible error than can be emitted while calling - vpCalibration member - */ - enum errorCodeEnum { - //! error returns by a constructor + * \brief Lists the possible error than can be emitted while calling + * vpCalibration member + */ + enum errorCodeEnum + { + //! Error returns by a constructor constructionError, - //! something is not initialized + //! Something is not initialized notInitializedError, - //! function not implemented + //! Function not implemented notImplementedError, - //! index out of range + //! Index out of range outOfRangeError, - //! iterative algorithm doesn't converge + //! Iterative algorithm doesn't converge convergencyError, + //! Forbidden operator forbiddenOperatorError, }; public: + /*! + * Constructor. + */ vpCalibrationException(int id, const char *format, ...) { this->code = id; @@ -96,7 +79,14 @@ class VISP_EXPORT vpCalibrationException : public vpException setMessage(format, args); va_end(args); } + /*! + * Constructor. + */ vpCalibrationException(int id, const std::string &msg) : vpException(id, msg) { } + + /*! + * Constructor. + */ explicit vpCalibrationException(int id) : vpException(id) { } }; diff --git a/modules/vision/include/visp3/vision/vpFernClassifier.h b/modules/vision/include/visp3/vision/vpFernClassifier.h index 70163a441c..dfa9b56ad4 100644 --- a/modules/vision/include/visp3/vision/vpFernClassifier.h +++ b/modules/vision/include/visp3/vision/vpFernClassifier.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -31,8 +30,7 @@ * Description: * Class that implements the Fern classifier and the YAPE detector thanks * to the OpenCV library. - * -*****************************************************************************/ + */ #ifndef vpFernClassifier_H #define vpFernClassifier_H diff --git a/modules/vision/include/visp3/vision/vpHandEyeCalibration.h b/modules/vision/include/visp3/vision/vpHandEyeCalibration.h index d266e4c8f6..15f166386a 100644 --- a/modules/vision/include/visp3/vision/vpHandEyeCalibration.h +++ b/modules/vision/include/visp3/vision/vpHandEyeCalibration.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Hand-eye calibration. - * - * Authors: - * Francois Chaumette - * -*****************************************************************************/ + */ /*! \file vpHandEyeCalibration.h @@ -52,16 +47,29 @@ #include /*! - \class vpHandEyeCalibration - - \ingroup group_vision_calib - - \brief Tool for hand-eye calibration. - -*/ + * \class vpHandEyeCalibration + * + * \ingroup group_vision_calib + * + * \brief Tool for hand-eye calibration. + */ class VISP_EXPORT vpHandEyeCalibration { public: + /*! + * Compute extrinsic camera parameters : the constant transformation from + * the effector to the camera frames (eMc). + * + * \param[in] cMo : vector of homogeneous matrices representing the transformation + * between the camera and the scene. + * \param[in] rMe : vector of homogeneous matrices representing the transformation + * between the effector (where the camera is fixed) and the reference + * coordinates (base of the manipulator). Must be the same size as cMo. + * \param[out] eMc : homogeneous matrix representing the transformation + * between the effector and the camera (output) + * + * \return 0 if calibration succeed, -1 if the system is not full rank, 1 if the algorithm doesn't converge. + */ static int calibrate(const std::vector &cMo, const std::vector &rMe, vpHomogeneousMatrix &eMc); diff --git a/modules/vision/include/visp3/vision/vpHomography.h b/modules/vision/include/visp3/vision/vpHomography.h index 84ad4011bb..4899cc6aef 100644 --- a/modules/vision/include/visp3/vision/vpHomography.h +++ b/modules/vision/include/visp3/vision/vpHomography.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,17 +29,13 @@ * * Description: * Homography transformation. - * - * Authors: - * Muriel Pressigout - * -*****************************************************************************/ + */ /*! -\file vpHomography.h + \file vpHomography.h -This file defines an homography transformation. This class aims to provide -some tools for homography computation. + This file defines an homography transformation. This class aims to provide + some tools for homography computation. */ #ifndef vpHomography_hh @@ -57,7 +52,6 @@ some tools for homography computation. #include /*! - \class vpHomography \ingroup group_vision_homography @@ -185,83 +179,274 @@ class VISP_EXPORT vpHomography : public vpArray2D //! build the homography from aMb and Rb void build(); - //! insert a rotation matrix + /*! + * Insert the rotational part of an homogeneous transformation. + * To recompute the homography call build(). + */ void insert(const vpHomogeneousMatrix &aRb); - //! insert a rotation matrix + + /*! + * Insert the rotational matrix. + * To recompute the homography call build(). + */ void insert(const vpRotationMatrix &aRb); - //! insert a theta u vector (transformation into a rotation matrix) + + /*! + * Insert a theta u vector transformed internally into a rotation matrix. + * To recompute the homography call build(). + */ void insert(const vpThetaUVector &tu); - //! insert a translation vector + + /*! + * Insert a translation vector. + * To recompute the homography call build(). + */ void insert(const vpTranslationVector &atb); - //! insert a translation vector + + /*! + * Insert the reference plane. + * To recompute the homography call build(). + */ void insert(const vpPlane &bP); static void initRansac(unsigned int n, double *xb, double *yb, double *xa, double *ya, vpColVector &x); public: + /*! + * Initialize an homography as identity. + */ vpHomography(); + /*! + * Initialize an homography from another homography. + */ vpHomography(const vpHomography &H); - //! Construction from Translation and rotation and a plane + //! Construction from translation and rotation and a plane. vpHomography(const vpHomogeneousMatrix &aMb, const vpPlane &bP); - //! Construction from Translation and rotation and a plane + //! Construction from translation and rotation and a plane. vpHomography(const vpRotationMatrix &aRb, const vpTranslationVector &atb, const vpPlane &bP); - //! Construction from Translation and rotation and a plane + //! Construction from translation and rotation and a plane. vpHomography(const vpThetaUVector &tu, const vpTranslationVector &atb, const vpPlane &bP); - //! Construction from Translation and rotation and a plane + //! Construction from translation and rotation and a plane. vpHomography(const vpPoseVector &arb, const vpPlane &bP); - virtual ~vpHomography(){}; + //! Destructor. + virtual ~vpHomography() { }; - //! Construction from Translation and rotation and a plane + //! Construction from translation and rotation and a plane void buildFrom(const vpRotationMatrix &aRb, const vpTranslationVector &atb, const vpPlane &bP); - //! Construction from Translation and rotation and a plane + //! Construction from translation and rotation and a plane void buildFrom(const vpThetaUVector &tu, const vpTranslationVector &atb, const vpPlane &bP); - //! Construction from Translation and rotation and a plane + //! Construction from translation and rotation and a plane void buildFrom(const vpPoseVector &arb, const vpPlane &bP); //! Construction from homogeneous matrix and a plane void buildFrom(const vpHomogeneousMatrix &aMb, const vpPlane &bP); + /*! + * Transform an homography from pixel space to calibrated domain. + * + * Given homography \f$\bf G\f$ corresponding to the collineation matrix in the pixel space, + * compute the homography matrix \f$\bf H\f$ in the Euclidean space or calibrated domain using: + * \f[ {\bf H} = {\bf K}^{-1} {\bf G} {\bf K} \f] + * \param[in] cam : Camera parameters used to fill \f${\bf K}\f$ matrix such as + * \f[{\bf K} = + * \left[ \begin{array}{ccc} + * p_x & 0 & u_0 \\ + * 0 & p_y & v_0 \\ + * 0 & 0 & 1 + * \end{array}\right] + * \f] + * \return The corresponding homography matrix \f$\bf H\f$ in the Euclidean space or calibrated domain. + * + * \sa homography2collineation() + */ vpHomography collineation2homography(const vpCameraParameters &cam) const; + /*! + * Converts an homography to a matrix. + * \return The 3x3 matrix corresponding to the homography. + */ vpMatrix convert() const; + /*! + * Compute the camera displacement between two images from the homography \f$ + * {^a}{\bf H}_b \f$ which is here an implicit parameter (*this). + * + * \param aRb : Rotation matrix as an output \f$ {^a}{\bf R}_b \f$. + * \param atb : Translation vector as an output \f$ ^a{\bf t}_b \f$. + * \param n : Normal vector to the plane as an output. + */ void computeDisplacement(vpRotationMatrix &aRb, vpTranslationVector &atb, vpColVector &n); + /*! + * Compute the camera displacement between two images from the homography \f$ + * {^a}{\bf H}_b \f$ which is here an implicit parameter (*this). + * + * Camera displacement between \f$ {^a}{\bf p} \f$ and \f$ {^a}{\bf p} \f$ is + * represented as a rotation matrix \f$ {^a}{\bf R}_b \f$ and a translation + * vector \f$ ^a{\bf t}_b \f$ from which an homogeneous matrix can be build + * (vpHomogeneousMatrix). + * + * \param nd : Input normal vector to the plane used to compar with the normal + * vector \e n extracted from the homography. + * \param aRb : Rotation matrix as an output \f$ {^a}{\bf R}_b \f$. + * \param atb : Translation vector as an output \f$ ^a{\bf t}_b \f$. + * \param n : Normal vector to the plane as an output. + */ void computeDisplacement(const vpColVector &nd, vpRotationMatrix &aRb, vpTranslationVector &atb, vpColVector &n); + /*! + * Return homography determinant. + */ double det() const; + + /*! + * Set the homography as identity transformation by setting the diagonal to 1 + * and all other values to 0. + */ void eye(); + /*! + * Transform an homography from calibrated domain to pixel space. + * + * Given homography \f$\bf H\f$ in the Euclidean space or in the calibrated domain, + * compute the homography \f$\bf G\f$ corresponding to the collineation matrix in the pixel space using: + * \f[ {\bf G} = {\bf K} {\bf H} {\bf K}^{-1} \f] + * \param[in] cam : Camera parameters used to fill \f${\bf K}\f$ matrix such as + * \f[{\bf K} = + * \left[ \begin{array}{ccc} + * p_x & 0 & u_0 \\ + * 0 & p_y & v_0 \\ + * 0 & 0 & 1 + * \end{array}\right] + * \f] + * \return The corresponding collineation matrix \f$\bf G\f$ in the pixel space. + * + * \sa collineation2homography() + */ vpHomography homography2collineation(const vpCameraParameters &cam) const; - //! invert the homography + /*! + * Return inverted homography. + * + * \param[in] sv_threshold : Threshold used to test the singular values. If + * a singular value is lower than this threshold we consider that the + * homography is not full rank. + * + * \param[out] rank : Rank of the homography that should be 3. + * + * \return \f$\bf H^{-1}\f$ + */ vpHomography inverse(double sv_threshold = 1e-16, unsigned int *rank = NULL) const; - //! invert the homography - void inverse(vpHomography &Hi) const; - //! Load an homography from a file + /*! + * Invert the homography. + * + * \param bHa : \f$\bf H^{-1}\f$ with H = *this. + */ + void inverse(vpHomography &bHa) const; + + /*! + * Read an homography in a file, verify if it is really an homogeneous + * matrix. + * + * \param f : the file. This file has to be written using save(). + * + * \sa save() + */ void load(std::ifstream &f); - // Multiplication by an homography + /*! + * Multiplication by an homography. + * + * \param H : Homography to multiply with. + * + * \code + * vpHomography aHb, bHc; + * // Initialize aHb and bHc homographies + * vpHomography aHc = aHb * bHc; + * \endcode + */ vpHomography operator*(const vpHomography &H) const; - // Multiplication by a scalar + + /*! + * Multiply an homography by a scalar. + * + * \param v : Value of the scalar. + * + * \code + * double v = 1.1; + * vpHomography aHb; + * // Initialize aHb + * vpHomography H = aHb * v; + * \endcode + */ vpHomography operator*(const double &v) const; + + /*! + * Operation a = aHb * b. + * + * \param b : 3 dimension vector. + */ vpColVector operator*(const vpColVector &b) const; - // Multiplication by a point - vpPoint operator*(const vpPoint &H) const; - // Division by a scalar + /*! + * 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 b_P : 2D coordinates of the point in the image plane b. + * + * \return A point with 2D coordinates in the image plane a. + */ + vpPoint operator*(const vpPoint &b_P) const; + + /*! + * Divide an homography by a scalar. + * + * \param v : Value of the scalar. + * + * \code + * vpHomography aHb; + * // Initialize aHb + * vpHomography H = aHb / aHb[2][2]; + * \endcode + */ vpHomography operator/(const double &v) const; + + /*! + * Divide all the element of the homography matrix by v : Hij = Hij / v + */ vpHomography &operator/=(double v); + + /*! + * Copy operator. + * Allow operation such as aHb = H + * + * \param H : Homography matrix to be copied. + */ vpHomography &operator=(const vpHomography &H); + + /*! + * Copy operator. + * Allow operation such as aHb = H + * + * \param H : Matrix to be copied. + */ vpHomography &operator=(const vpMatrix &H); - vpImagePoint projection(const vpImagePoint &p); + /*! + * Project the current image point (in frame b) into the frame a using the + * homography aHb. + * + * \param ipb : Homography defining the relation between frame a and frame b. + * \return The projected image point in the frame a. + */ + vpImagePoint projection(const vpImagePoint &ipb); /*! - This function is not applicable to an homography that is always a - 3-by-3 matrix. - \exception vpException::fatalError When this function is called. - */ + * This function is not applicable to an homography that is always a + * 3-by-3 matrix. + * \exception vpException::fatalError When this function is called. + */ void resize(unsigned int nrows, unsigned int ncols, bool flagNullify = true) { (void)nrows; @@ -270,23 +455,206 @@ class VISP_EXPORT vpHomography : public vpArray2D throw(vpException(vpException::fatalError, "Cannot resize an homography matrix")); }; + /*! + * Save an homography in a file. + * The load() function allows then to read and set the homography from this + * file. + * + * \sa load() + */ void save(std::ofstream &f) const; + /*! + * From couples of matched points \f$^a{\bf p}=(x_a,y_a,1)\f$ in image a + * and \f$^b{\bf p}=(x_b,y_b,1)\f$ in image b with homogeneous coordinates, + * computes the homography matrix by resolving \f$^a{\bf p} = ^a{\bf H}_b\; + * ^b{\bf p}\f$ using the DLT (Direct Linear Transform) algorithm. + * + * At least 4 couples of points are needed. + * + * To do so, we use the DLT algorithm on the data, + * ie we resolve the linear system by SDV : \f$\bf{Ah} =0\f$ where + * \f$\bf{h}\f$ is the vector with the terms of \f$^a{\bf H}_b\f$ and + * \f$\mathbf{A}\f$ depends on the points coordinates. + * + * For each point, in homogeneous coordinates we have: + * \f[ + * ^a{\bf p} = ^a{\bf H}_b\; ^b{\bf p} + * \f] + * which is equivalent to: + * \f[ + * ^a{\bf p} \times {^a{\bf H}_b \; ^b{\bf p}} =0 + * \f] + * If we note \f$\mathbf{h}_j^T\f$ the \f$j^{\textrm{th}}\f$ line of + * \f$^a{\bf H}_b\f$, we can write: \f[ ^a{\bf H}_b \; ^b{\bf p} = \left( + * \begin{array}{c}\mathbf{h}_1^T \;^b{\bf p} \\\mathbf{h}_2^T \; ^b{\bf p} + * \\\mathbf{h}_3^T \;^b{\bf p} \end{array}\right) \f] + * + * Setting \f$^a{\bf p}=(x_{a},y_{a},w_{a})\f$, the cross product can be + * rewritten by: \f[ ^a{\bf p} \times ^a{\bf H}_b \; ^b{\bf p} =\left( + * \begin{array}{c}y_{a}\mathbf{h}_3^T \; ^b{\bf p}-w_{a}\mathbf{h}_2^T \; + * ^b{\bf p} \\w_{a}\mathbf{h}_1^T \; ^b{\bf p} -x_{a}\mathbf{h}_3^T \; ^b{\bf + * p} \\x_{a}\mathbf{h}_2^T \; ^b{\bf p}- y_{a}\mathbf{h}_1^T \; ^b{\bf + * p}\end{array}\right) \f] + * + * \f[ + * \underbrace{\left( \begin{array}{ccc}\mathbf{0}^T & -w_{a} \; ^b{\bf p}^T + * & y_{a} \; ^b{\bf p}^T \\ w_{a} + * \; ^b{\bf p}^T&\mathbf{0}^T & -x_{a} \; ^b{\bf p}^T \\ + * -y_{a} \; ^b{\bf p}^T & x_{a} \; ^b{\bf p}^T & + * \mathbf{0}^T\end{array}\right)}_{\mathbf{A}_i (3\times 9)} + * \underbrace{\left( \begin{array}{c}\mathbf{h}_{1}^{T} \\ + * \mathbf{h}_{2}^{T}\\\mathbf{h}_{3}^{T}\end{array}\right)}_{\mathbf{h} + * (9\times 1)}=0 \f] + * + * leading to an homogeneous system to be solved: + * \f$\mathbf{A}\mathbf{h}=0\f$ with \f$\mathbf{A}=\left(\mathbf{A}_1^T, ..., + * \mathbf{A}_i^T, ..., \mathbf{A}_n^T \right)^T\f$. + * + * It can be solved using an SVD decomposition: + * \f[\bf A = UDV^T \f] + * h is the column of V associated with the smallest singular + * value of A + * + * + * \param xb, yb : Coordinates vector of matched points in image b. These + * coordinates are expressed in meters. + * \param xa, ya : Coordinates vector of + * matched points in image a. These coordinates are expressed in meters. + * \param + * aHb : Estimated homography that relies the transformation from image a to + * image b. + * \param normalization : When set to true, the coordinates of the + * points are normalized. The normalization carried out is the one preconized + * by Hartley. + * + * \exception vpMatrixException::rankDeficient : When the rank of the matrix + * that should be 8 is deficient. + */ static void DLT(const std::vector &xb, const std::vector &yb, const std::vector &xa, const std::vector &ya, vpHomography &aHb, bool normalization = true); + /*! + * From couples of matched points \f$^a{\bf p}=(x_a,y_a,1)\f$ in image a + * and \f$^b{\bf p}=(x_b,y_b,1)\f$ in image b with homogeneous coordinates, + * computes the homography matrix by resolving \f$^a{\bf p} = ^a{\bf H}_b\; + * ^b{\bf p}\f$ using Ezio Malis linear method (HLM) \cite Malis00b. + * + * This method can consider points that are planar or non planar. The algorithm + * for planar scene implemented in this file is described in Ezio Malis PhD + * thesis \cite TheseMalis. + * + * \param xb, yb : Coordinates vector of matched points in image b. These + * coordinates are expressed in meters. + * \param xa, ya : Coordinates vector of + * matched points in image a. These coordinates are expressed in meters. + * \param isplanar : If true the points are assumed to be in a plane, otherwise there + * are assumed to be non planar. + * \param aHb : Estimated homography that relies + * the transformation from image a to image b. + * + * If the boolean isplanar is true the points are assumed to be in a plane + * otherwise there are assumed to be non planar. + * + * \sa DLT() when the scene is planar. + */ static void HLM(const std::vector &xb, const std::vector &yb, const std::vector &xa, const std::vector &ya, bool isplanar, vpHomography &aHb); + /*! + * From couples of matched points \f$^a{\bf p}=(x_a,y_a,1)\f$ in image a + * and \f$^b{\bf p}=(x_b,y_b,1)\f$ in image b with homogeneous coordinates, + * computes the homography matrix by resolving \f$^a{\bf p} = ^a{\bf H}_b\; + * ^b{\bf p}\f$ using Ransac algorithm. + * + * \param xb, yb : Coordinates vector of matched points in image b. These + * coordinates are expressed in meters. + * \param xa, ya : Coordinates vector of + * matched points in image a. These coordinates are expressed in meters. + * \param aHb : Estimated homography that relies the transformation from image a to + * image b. + * \param inliers : Vector that indicates if a matched point is an + * inlier (true) or an outlier (false). + * \param residual : Global residual + * computed as \f$r = \sqrt{1/n \sum_{inliers} {\| {^a{\bf p} - {\hat{^a{\bf + * H}_b}} {^b{\bf p}}} \|}^{2}}\f$ with \f$n\f$ the number of inliers. + * \param nbInliersConsensus : Minimal number of points requested to fit the + * estimated homography. + * \param threshold : Threshold for outlier removing. A point is considered as + * an outlier if the reprojection error \f$\| {^a{\bf p} - {\hat{^a{\bf H}_b}} + * {^b{\bf p}}} \|\f$ is greater than this threshold. + * \param normalization : When set to true, the coordinates of the points are + * normalized. The normalization carried out is the one preconized by Hartley. + * + * \return true if the homography could be computed, false otherwise. + */ static bool ransac(const std::vector &xb, const std::vector &yb, const std::vector &xa, const std::vector &ya, vpHomography &aHb, std::vector &inliers, double &residual, unsigned int nbInliersConsensus, double threshold, bool normalization = true); + /*! + * Given `iPa` a pixel with coordinates \f$(u_a,v_a)\f$ in + * image a, and the homography `bHa` in the Euclidean space or calibrated domain that links image a and b, computes the + * coordinates of the pixel \f$(u_b,v_b)\f$ in the image b using the camera + * parameters matrix \f$\bf K\f$. + * + * Compute \f$^b{\bf p} = {\bf K} \; {^b}{\bf H}_a \; {\bf K}^{-1} {^a}{\bf + * p}\f$ with \f$^a{\bf p}=(u_a,v_a,1)\f$ and \f$^b{\bf p}=(u_b,v_b,1)\f$ + * + * \return The coordinates in pixel of the point with coordinates + * \f$(u_b,v_b)\f$. + */ static vpImagePoint project(const vpCameraParameters &cam, const vpHomography &bHa, const vpImagePoint &iPa); + + /*! + * Given `Pa` a point with normalized coordinates \f$(x_a,y_a,1)\f$ in the + * image plane a, and the homography `bHa` in the Euclidean space that links image a and b, computes + * the normalized coordinates of the point \f$(x_b,y_b,1)\f$ in the image plane + * b. + * + * Compute \f$^b{\bf p} = {^b}{\bf H}_a \; {^a}{\bf p}\f$ with \f$^a{\bf + * p}=(x_a,y_a,1)\f$ and \f$^b{\bf p}=(x_b,y_b,1)\f$ + * + * \return The coordinates in meter of the point with coordinates + * \f$(x_b,y_b)\f$. + */ static vpPoint project(const vpHomography &bHa, const vpPoint &Pa); + /*! + * From couples of matched points \f$^a{\bf p}=(x_a,y_a,1)\f$ in image a + * and \f$^b{\bf p}=(x_b,y_b,1)\f$ in image b with homogeneous coordinates, + * computes the homography matrix by resolving \f$^a{\bf p} = ^a{\bf H}_b\; + * ^b{\bf p}\f$ using a robust estimation scheme. + * + * This method is to compare to DLT() except that here a robust estimator is + * used to reject couples of points that are considered as outliers. + * + * At least 4 couples of points are needed. + * + * \param xb, yb : Coordinates vector of matched points in image b. These + * coordinates are expressed in meters. + * \param xa, ya : Coordinates vector of + * matched points in image a. These coordinates are expressed in meters. + * \param aHb : Estimated homography that relies the transformation from image a to + * image b. + * \param inliers : Vector that indicates if a matched point is an + * inlier (true) or an outlier (false). + * \param residual : Global residual computed as + * \f$r = \sqrt{1/n \sum_{inliers} {\| {^a{\bf p} - {\hat{^a{\bf H}_b}} {^b{\bf p}}} \|}^{2}}\f$ + * with \f$n\f$ the number of inliers. + * \param weights_threshold : Threshold applied on the weights updated during the + * robust estimation and used to consider if a point is an outlier or an + * inlier. Values should be in [0:1]. A couple of matched points that have a + * weight lower than this threshold is considered as an outlier. A value equal + * to zero indicates that all the points are inliers. + * \param niter : Number of iterations of the estimation process. + * \param normalization : When set to true, the coordinates of the points are normalized. + * The normalization carried out is the one preconized by Hartley. + * + * \sa DLT(), ransac() + */ static void robust(const std::vector &xb, const std::vector &yb, const std::vector &xa, - const std::vector &ya, vpHomography &aHb, std::vector &inlier, double &residual, + const std::vector &ya, vpHomography &aHb, std::vector &inliers, double &residual, double weights_threshold = 0.4, unsigned int niter = 4, bool normalization = true); #ifndef DOXYGEN_SHOULD_SKIP_THIS diff --git a/modules/vision/include/visp3/vision/vpKeyPoint.h b/modules/vision/include/visp3/vision/vpKeyPoint.h index a965468f70..6b84e74442 100644 --- a/modules/vision/include/visp3/vision/vpKeyPoint.h +++ b/modules/vision/include/visp3/vision/vpKeyPoint.h @@ -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: * Key point functionalities. - * -*****************************************************************************/ + */ #ifndef _vpKeyPoint_h_ #define _vpKeyPoint_h_ diff --git a/modules/vision/include/visp3/vision/vpLevenbergMarquartd.h b/modules/vision/include/visp3/vision/vpLevenbergMarquartd.h index 4239f203cd..114d0f73d2 100644 --- a/modules/vision/include/visp3/vision/vpLevenbergMarquartd.h +++ b/modules/vision/include/visp3/vision/vpLevenbergMarquartd.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Levenberg Marquartd. - * - * Authors: - * Francois Chaumette - * -*****************************************************************************/ + */ #ifndef vpLevenbergMarquartd_h #define vpLevenbergMarquartd_h @@ -75,9 +70,3 @@ int VISP_EXPORT lmder1(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, int *ipvt, int lwa, double *wa); #endif - -/* - * Local variables: - * c-basic-offset: 2 - * End: - */ diff --git a/modules/vision/include/visp3/vision/vpPlanarObjectDetector.h b/modules/vision/include/visp3/vision/vpPlanarObjectDetector.h index 311a8ad6ce..ea191ea62c 100644 --- a/modules/vision/include/visp3/vision/vpPlanarObjectDetector.h +++ b/modules/vision/include/visp3/vision/vpPlanarObjectDetector.h @@ -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: * Planar surface detection tool. - * -*****************************************************************************/ + */ #ifndef _vpPlanarObjectDetector_h_ #define _vpPlanarObjectDetector_h_ diff --git a/modules/vision/include/visp3/vision/vpPlaneEstimation.h b/modules/vision/include/visp3/vision/vpPlaneEstimation.h index 49f64d6789..3841739feb 100644 --- a/modules/vision/include/visp3/vision/vpPlaneEstimation.h +++ b/modules/vision/include/visp3/vision/vpPlaneEstimation.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Plane estimation. - * - * Authors: - * Julien Dufour - * -*****************************************************************************/ + */ /*! \file vpPlaneEstimation.h @@ -48,7 +43,7 @@ #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \ (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911))) -// Visual Studio: Optionals are available from Visual Studio 2017 RTW (15.0) [1910] +// Visual Studio: Optionals are available from Visual Studio 2017 RTW (15.0) [1910] // Visual Studio: Structured bindings are available from Visual Studio 2017 version 15.3 [1911] (cf .cpp) // System diff --git a/modules/vision/include/visp3/vision/vpPose.h b/modules/vision/include/visp3/vision/vpPose.h index fd5287a4a9..e387d849a1 100644 --- a/modules/vision/include/visp3/vision/vpPose.h +++ b/modules/vision/include/visp3/vision/vpPose.h @@ -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: * Pose computation. - * -*****************************************************************************/ + */ /*! \file vpPose.h diff --git a/modules/vision/include/visp3/vision/vpPoseException.h b/modules/vision/include/visp3/vision/vpPoseException.h index e626cc3f0b..57be1d404d 100644 --- a/modules/vision/include/visp3/vision/vpPoseException.h +++ b/modules/vision/include/visp3/vision/vpPoseException.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,57 +29,46 @@ * * Description: * Error that can be emitted by the vpPose class and its derivatives - * -*****************************************************************************/ + */ #ifndef _vpPoseException_h_ #define _vpPoseException_h_ -/* ------------------------------------------------------------------------- - */ -/* --- INCLUDE ------------------------------------------------------------- - */ -/* ------------------------------------------------------------------------- - */ - -/* Classes standards. */ - #include -#include /* Classe std::ostream. */ -#include /* Classe string. */ - -/* ------------------------------------------------------------------------- - */ -/* --- CLASS --------------------------------------------------------------- - */ -/* ------------------------------------------------------------------------- - */ +#include +#include /*! - \class vpPoseException - \ingroup group_vision_pose - \brief Error that can be emitted by the vpPose class and its derivatives. + * \class vpPoseException + * \ingroup group_vision_pose + * \brief Error that can be emitted by the vpPose class and its derivatives. */ class VISP_EXPORT vpPoseException : public vpException { public: /*! - \brief Lists the possible error than can be emitted while calling - vpPose member - */ - enum errorCodeEnum { + * Lists the possible error than can be emitted while calling + * vpPose member + */ + enum errorCodeEnum + { + //! Generic pose error poseError, - //! something is not initialized + //! Something is not initialized notInitializedError, - //! function not implemented + //! Function not implemented notImplementedERR, - //! index out of range + //! Index out of range outOfRangeError, + //! Not enough points to compute the pose notEnoughPointError }; public: + /*! + * Constructor. + */ vpPoseException(int id, const char *format, ...) { this->code = id; @@ -89,9 +77,16 @@ class VISP_EXPORT vpPoseException : public vpException setMessage(format, args); va_end(args); } + + /*! + * Constructor. + */ vpPoseException(int id, const std::string &msg) : vpException(id, msg) { } + + /*! + * Constructor. + */ explicit vpPoseException(int id) : vpException(id) { } - // vpPoseException() : vpException() { ;} }; #endif diff --git a/modules/vision/include/visp3/vision/vpPoseFeatures.h b/modules/vision/include/visp3/vision/vpPoseFeatures.h index 737470b921..c77cc66663 100644 --- a/modules/vision/include/visp3/vision/vpPoseFeatures.h +++ b/modules/vision/include/visp3/vision/vpPoseFeatures.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Pose computation from any features. - * - * Authors: - * Aurelien Yol - * -*****************************************************************************/ + */ /*! \file vpPose.h diff --git a/modules/vision/include/visp3/vision/vpXmlConfigParserKeyPoint.h b/modules/vision/include/visp3/vision/vpXmlConfigParserKeyPoint.h index a1608f7c14..3d838826dc 100644 --- a/modules/vision/include/visp3/vision/vpXmlConfigParserKeyPoint.h +++ b/modules/vision/include/visp3/vision/vpXmlConfigParserKeyPoint.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * XML parser to load configuration for vpKeyPoint class. - * - * Authors: - * Souriya Trinh - * -*****************************************************************************/ + */ /*! \file vpXmlConfigParserKeyPoint.cpp diff --git a/modules/vision/src/calibration/vpCalibration.cpp b/modules/vision/src/calibration/vpCalibration.cpp index 6d16509ff2..506a0937de 100644 --- a/modules/vision/src/calibration/vpCalibration.cpp +++ b/modules/vision/src/calibration/vpCalibration.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,12 +29,7 @@ * * Description: * Camera calibration. - * - * Authors: - * Francois Chaumette - * Anthony Saunier - * -*****************************************************************************/ + */ /*! \file vpCalibration.cpp @@ -50,9 +44,7 @@ double vpCalibration::threshold = 1e-10f; unsigned int vpCalibration::nbIterMax = 4000; double vpCalibration::gain = 0.25; -/*! - Basic initialisation (called by the constructors) -*/ + int vpCalibration::init() { npt = 0; @@ -67,36 +59,23 @@ int vpCalibration::init() return 0; } -/*! - Default constructor. - */ vpCalibration::vpCalibration() : cMo(), cMo_dist(), cam(), cam_dist(), rMe(), eMc(), eMc_dist(), m_aspect_ratio(-1), npt(0), LoX(), LoY(), LoZ(), - Lip(), residual(1000.), residual_dist(1000.) + Lip(), residual(1000.), residual_dist(1000.) { init(); } -/*! - Copy constructor. - */ + vpCalibration::vpCalibration(const vpCalibration &c) : cMo(), cMo_dist(), cam(), cam_dist(), rMe(), eMc(), eMc_dist(), m_aspect_ratio(-1), npt(0), LoX(), LoY(), LoZ(), - Lip(), residual(1000.), residual_dist(1000.) + Lip(), residual(1000.), residual_dist(1000.) { (*this) = c; } -/*! - Destructor : delete the array of point (freed the memory) -*/ vpCalibration::~vpCalibration() { clearPoint(); } -/*! - = operator. - - \param twinCalibration : object to be copied -*/ vpCalibration &vpCalibration::operator=(const vpCalibration &twinCalibration) { npt = twinCalibration.npt; @@ -123,9 +102,6 @@ vpCalibration &vpCalibration::operator=(const vpCalibration &twinCalibration) return (*this); } -/*! - Delete the array of points. -*/ int vpCalibration::clearPoint() { LoX.clear(); @@ -137,12 +113,6 @@ int vpCalibration::clearPoint() return 0; } -/*! - - Add a new point in the array of points. - \param X,Y,Z : 3D coordinates of a point in the object frame - \param ip : 2D Coordinates of the point in the camera frame. -*/ int vpCalibration::addPoint(double X, double Y, double Z, vpImagePoint &ip) { LoX.push_back(X); @@ -193,13 +163,6 @@ void vpCalibration::computePose(const vpCameraParameters &camera, vpHomogeneousM pose.computePose(vpPose::DEMENTHON_LAGRANGE_VIRTUAL_VS, cMo_est); } -/*! - Compute and return the standard deviation expressed in pixel - for pose matrix and camera intrinsic parameters for model without - distortion. \param cMo_est : the matrix that defines the pose to be tested. - \param camera : camera intrinsic parameters to be tested. - \return the standard deviation by point of the error in pixel . -*/ double vpCalibration::computeStdDeviation(const vpHomogeneousMatrix &cMo_est, const vpCameraParameters &camera) { double residual_ = 0; @@ -244,13 +207,7 @@ double vpCalibration::computeStdDeviation(const vpHomogeneousMatrix &cMo_est, co this->residual = residual_; return sqrt(residual_ / npt); } -/*! - Compute and return the standard deviation expressed in pixel - for pose matrix and camera intrinsic parameters with pixel to meter model. - \param cMo_est : the matrix that defines the pose to be tested. - \param camera : camera intrinsic parameters to be tested. - \return the standard deviation by point of the error in pixel . -*/ + double vpCalibration::computeStdDeviation_dist(const vpHomogeneousMatrix &cMo_est, const vpCameraParameters &camera) { double residual_ = 0; @@ -311,30 +268,12 @@ double vpCalibration::computeStdDeviation_dist(const vpHomogeneousMatrix &cMo_es return sqrt(residual_ / npt); } -/*! - Compute and return the standard deviation expressed in pixel - for pose matrix and camera intrinsic parameters. - \param deviation : the standard deviation computed for the model without - distortion. \param deviation_dist : the standard deviation computed for the - model with distortion. -*/ void vpCalibration::computeStdDeviation(double &deviation, double &deviation_dist) { deviation = computeStdDeviation(cMo, cam); deviation_dist = computeStdDeviation_dist(cMo_dist, cam_dist); } -/*! - Compute the calibration according to the desired method using one pose. - - \param method : Method that will be used to estimate the parameters. - \param cMo_est : estimated homogeneous matrix that defines the pose. - \param cam_est : estimated intrinsic camera parameters. - \param verbose : set at true if information about the residual at each loop - of the algorithm is hoped. - - \return EXIT_SUCCESS if the calibration succeed, EXIT_FAILURE otherwise. -*/ int vpCalibration::computeCalibration(vpCalibrationMethodType method, vpHomogeneousMatrix &cMo_est, vpCameraParameters &cam_est, bool verbose) { @@ -414,25 +353,12 @@ int vpCalibration::computeCalibration(vpCalibrationMethodType method, vpHomogene } return EXIT_SUCCESS; - } catch (...) { + } + catch (...) { throw; } } -/*! - Compute the multi-images calibration according to the desired method using - many poses. - - \param method : Method used to estimate the camera parameters. - \param table_cal : Vector of vpCalibration. - \param cam_est : Estimated intrinsic camera parameters. - \param globalReprojectionError : Global reprojection error or global - residual. - \param verbose : Set at true if information about the residual at - each loop of the algorithm is hoped. - - \return EXIT_SUCCESS if the calibration succeed, EXIT_FAILURE otherwise. -*/ int vpCalibration::computeCalibrationMulti(vpCalibrationMethodType method, std::vector &table_cal, vpCameraParameters &cam_est, double &globalReprojectionError, bool verbose) { @@ -446,9 +372,10 @@ int vpCalibration::computeCalibrationMulti(vpCalibrationMethodType method, std:: case CALIB_LAGRANGE: { if (nbPose > 1) { std::cout << "this calibration method is not available in" << std::endl - << "vpCalibration::computeCalibrationMulti()" << std::endl; + << "vpCalibration::computeCalibrationMulti()" << std::endl; return -1; - } else { + } + else { table_cal[0].calibLagrange(cam_est, table_cal[0].cMo); table_cal[0].cam = cam_est; table_cal[0].cam_dist = cam_est; @@ -460,10 +387,11 @@ int vpCalibration::computeCalibrationMulti(vpCalibrationMethodType method, std:: case CALIB_LAGRANGE_VIRTUAL_VS_DIST: { if (nbPose > 1) { std::cout << "this calibration method is not available in" << std::endl - << "vpCalibration::computeCalibrationMulti()" << std::endl - << "with several images." << std::endl; + << "vpCalibration::computeCalibrationMulti()" << std::endl + << "with several images." << std::endl; return -1; - } else { + } + else { table_cal[0].calibLagrange(cam_est, table_cal[0].cMo); table_cal[0].cam = cam_est; table_cal[0].cam_dist = cam_est; @@ -517,21 +445,15 @@ int vpCalibration::computeCalibrationMulti(vpCalibrationMethodType method, std:: } return EXIT_SUCCESS; - } catch (...) { + } + catch (...) { throw; } } -/*! - Write data into a file. - - data are organized as follow oX oY oZ u v - - \param filename : name of the file -*/ -int vpCalibration::writeData(const char *filename) +int vpCalibration::writeData(const std::string &filename) { - std::ofstream f(filename); + std::ofstream f(filename.c_str()); vpImagePoint ip; std::list::const_iterator it_LoX = LoX.begin(); @@ -565,17 +487,11 @@ int vpCalibration::writeData(const char *filename) return 0; } -/*! - Read data from disk : - data are organized as follow oX oY oZ u v - - \param filename : name of the file -*/ -int vpCalibration::readData(const char *filename) +int vpCalibration::readData(const std::string &filename) { vpImagePoint ip; std::ifstream f; - f.open(filename); + f.open(filename.c_str()); if (!f.fail()) { unsigned int n; f >> n; @@ -597,32 +513,18 @@ int vpCalibration::readData(const char *filename) f.close(); return 0; - } else { + } + else { return -1; } } -/*! - Read calibration grid coordinates from disk : - data are organized as follow oX oY oZ - \param filename : name of the file - \param n : number of points in the calibration grid - \param oX : List of oX coordinates - \param oY : List of oY coordinates - \param oZ : List of oZ coordinates - - \param verbose : Additionnal printings if true (number of points on - the calibration grid and their respective coordinates in the object - frame). - - \return 0 if success, -1 if an error occurs. -*/ -int vpCalibration::readGrid(const char *filename, unsigned int &n, std::list &oX, std::list &oY, +int vpCalibration::readGrid(const std::string &filename, unsigned int &n, std::list &oX, std::list &oY, std::list &oZ, bool verbose) { try { std::ifstream f; - f.open(filename); + f.open(filename.c_str()); if (!f.fail()) { f >> n; @@ -651,25 +553,17 @@ int vpCalibration::readGrid(const char *filename, unsigned int &n, std::list &I, vpColor color, unsigned int thickness, int subsampling_factor) { @@ -684,16 +578,6 @@ int vpCalibration::displayData(vpImage &I, vpColor color, unsigne return 0; } -/*! - Display estimated centers of dots using intrinsic camera parameters - with model with distortion and the computed pose. - \param I : Image where to display grid data. - \param color : Color of the data. - \param thickness : Thickness of the displayed data. - \param subsampling_factor : Subsampling factor. Default value is 1. - Admissible values are multiple of 2. Divide by this parameter the - values of the camera parameters. -*/ int vpCalibration::displayGrid(vpImage &I, vpColor color, unsigned int thickness, int subsampling_factor) { double u0_dist = cam_dist.get_u0() / subsampling_factor; @@ -717,20 +601,6 @@ int vpCalibration::displayGrid(vpImage &I, vpColor color, unsigne double oY = *it_LoY; double oZ = *it_LoZ; - // double cX = oX*cMo[0][0]+oY*cMo[0][1]+oZ*cMo[0][2] + cMo[0][3]; - // double cY = oX*cMo[1][0]+oY*cMo[1][1]+oZ*cMo[1][2] + cMo[1][3]; - // double cZ = oX*cMo[2][0]+oY*cMo[2][1]+oZ*cMo[2][2] + cMo[2][3]; - - // double x = cX/cZ ; - // double y = cY/cZ ; - - // double xp = u0 + x*px ; - // double yp = v0 + y*py ; - - // vpDisplay::displayCross(I,(int)vpMath::round(yp), - // (int)vpMath::round(xp), - // 5,col) ; - double cX = oX * cMo_dist[0][0] + oY * cMo_dist[0][1] + oZ * cMo_dist[0][2] + cMo_dist[0][3]; double cY = oX * cMo_dist[1][0] + oY * cMo_dist[1][1] + oZ * cMo_dist[1][2] + cMo_dist[1][3]; double cZ = oX * cMo_dist[2][0] + oY * cMo_dist[2][1] + oZ * cMo_dist[2][2] + cMo_dist[2][3]; @@ -756,11 +626,6 @@ int vpCalibration::displayGrid(vpImage &I, vpColor color, unsigne return 0; } -/*! - * Set pixel aspect ratio px/py. - * \param[in] aspect_ratio : px/py aspect ratio. Value need to be positive. - * To estimate a model where px=py set 1 as aspect ratio. - */ void vpCalibration::setAspectRatio(double aspect_ratio) { if (aspect_ratio > 0.) { diff --git a/modules/vision/src/calibration/vpCalibrationTools.cpp b/modules/vision/src/calibration/vpCalibrationTools.cpp index cfabf1d1c3..787751ed48 100644 --- a/modules/vision/src/calibration/vpCalibrationTools.cpp +++ b/modules/vision/src/calibration/vpCalibrationTools.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Camera calibration. - * - * Authors: - * Francois Chaumette - * -*****************************************************************************/ + */ #include #include diff --git a/modules/vision/src/calibration/vpHandEyeCalibration.cpp b/modules/vision/src/calibration/vpHandEyeCalibration.cpp index d7964f3904..77b7155eab 100644 --- a/modules/vision/src/calibration/vpHandEyeCalibration.cpp +++ b/modules/vision/src/calibration/vpHandEyeCalibration.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Hand-eye calibration. - * - * Authors: - * Francois Chaumette - * -*****************************************************************************/ + */ #include // std::fabs #include // numeric_limits @@ -400,19 +395,6 @@ int vpHandEyeCalibration::calibrationRotationTsaiOld(const std::vector } // end while #if DEBUG_LEVEL2 { - printf(" Iteration number for NL hand-eye minimisation : %d\n", it); + printf(" Iteration number for NL hand-eye minimization : %d\n", it); vpThetaUVector ePc(eRc); std::cout << "theta U (deg): " << vpMath::deg(ePc[0]) << " " << vpMath::deg(ePc[1]) << " " << vpMath::deg(ePc[2]) << std::endl; @@ -835,20 +817,6 @@ int vpHandEyeCalibration::calibrationVVS(const std::vector #define HE_PROCRUSTES_OT 5 #define HE_PROCRUSTES_NT 6 -/*! - Compute extrinsic camera parameters : the constant transformation from - the effector to the camera frames (eMc). - - \param[in] cMo : vector of homogeneous matrices representing the transformation - between the camera and the scene. - \param[in] rMe : vector of homogeneous matrices representing the transformation - between the effector (where the camera is fixed) and the reference - coordinates (base of the manipulator). Must be the same size as cMo. - \param[out] eMc : homogeneous matrix representing the transformation - between the effector and the camera (output) - - \return 0 if calibration succeed, -1 if the system is not full rank, 1 if the algorithm doesn't converge. -*/ int vpHandEyeCalibration::calibrate(const std::vector &cMo, const std::vector &rMe, vpHomogeneousMatrix &eMc) { diff --git a/modules/vision/src/homography-estimation/vpHomography.cpp b/modules/vision/src/homography-estimation/vpHomography.cpp index dc4f5f03dc..bd4a136e9c 100644 --- a/modules/vision/src/homography-estimation/vpHomography.cpp +++ b/modules/vision/src/homography-estimation/vpHomography.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Homography transformation. - * - * Authors: - * Muriel Pressigout - * -*****************************************************************************/ + */ /*! \file vpHomography.cpp @@ -53,23 +48,13 @@ #include #include -/*! - \brief initialize an homography as Identity -*/ vpHomography::vpHomography() : vpArray2D(3, 3), aMb(), bP() { eye(); } -/*! - \brief initialize an homography from another homography -*/ - vpHomography::vpHomography(const vpHomography &H) : vpArray2D(3, 3), aMb(), bP() { *this = H; } -/*! - \brief initialize an homography from another homography -*/ -vpHomography::vpHomography(const vpHomogeneousMatrix &M, const vpPlane &p) : vpArray2D(3, 3), aMb(), bP() +vpHomography::vpHomography(const vpHomogeneousMatrix &aMb, const vpPlane &bP) : vpArray2D(3, 3), aMb(), bP() { - buildFrom(M, p); + buildFrom(aMb, bP); } vpHomography::vpHomography(const vpThetaUVector &tu, const vpTranslationVector &atb, const vpPlane &p) @@ -89,10 +74,10 @@ vpHomography::vpHomography(const vpPoseVector &arb, const vpPlane &p) : vpArray2 buildFrom(arb, p); } -void vpHomography::buildFrom(const vpHomogeneousMatrix &M, const vpPlane &p) +void vpHomography::buildFrom(const vpHomogeneousMatrix &aMb, const vpPlane &bP) { - insert(M); - insert(p); + insert(aMb); + insert(bP); build(); } @@ -121,52 +106,20 @@ void vpHomography::buildFrom(const vpPoseVector &arb, const vpPlane &p) /*********************************************************************/ -/*! - \brief insert the rotational component. - To recompute the homography call build(). -*/ void vpHomography::insert(const vpRotationMatrix &aRb) { aMb.insert(aRb); } -/*! - \brief insert the rotational component. - To recompute the homography call build(). -*/ void vpHomography::insert(const vpHomogeneousMatrix &M) { this->aMb = M; } -/*! \brief insert the rotational component, insert a - theta u vector (transformation into a rotation matrix). - To recompute the homography call build(). - -*/ void vpHomography::insert(const vpThetaUVector &tu) { vpRotationMatrix aRb(tu); aMb.insert(aRb); } -/*! - \brief insert the translational component in a homography. - To recompute the homography call build(). -*/ void vpHomography::insert(const vpTranslationVector &atb) { aMb.insert(atb); } -/*! - \brief insert the reference plane. - To recompute the homography call build(). -*/ void vpHomography::insert(const vpPlane &p) { this->bP = p; } -/*! - Return inverted homography. - - \param[in] sv_threshold : Threshold used to test the singular values. If - a singular value is lower than this threshold we consider that the - homography is not full rank. - - \param[out] rank : Rank of the homography that should be 3. - - \return \f$\bf H^{-1}\f$ -*/ vpHomography vpHomography::inverse(double sv_threshold, unsigned int *rank) const { vpMatrix M = (*this).convert(); @@ -185,20 +138,8 @@ vpHomography vpHomography::inverse(double sv_threshold, unsigned int *rank) cons return H; } -/*! - \brief Invert the homography. - - \param bHa : \f$\bf H^{-1}\f$ with H = *this. -*/ void vpHomography::inverse(vpHomography &bHa) const { bHa = inverse(); } -/*! - Save an homography in a file. - The laod() function allows then to read and set the homography from this - file. - - \sa load() - */ void vpHomography::save(std::ofstream &f) const { if (!f.fail()) { @@ -208,19 +149,6 @@ void vpHomography::save(std::ofstream &f) const } } -/*! - - Multiplication by an homography. - - \param H : Homography to multiply with. - - \code - vpHomography aHb, bHc; - // Initialize aHb and bHc homographies - vpHomography aHc = aHb * bHc; - \endcode - -*/ vpHomography vpHomography::operator*(const vpHomography &H) const { vpHomography Hp; @@ -236,11 +164,6 @@ vpHomography vpHomography::operator*(const vpHomography &H) const return Hp; } -/*! - Operation a = aHb * b. - - \param b : 3 dimension vector. -*/ vpColVector vpHomography::operator*(const vpColVector &b) const { if (b.size() != 3) @@ -257,20 +180,6 @@ vpColVector vpHomography::operator*(const vpColVector &b) const return a; } -/*! - - Multiply an homography by a scalar. - - \param v : Value of the scalar. - - \code - double v = 1.1; - vpHomography aHb; - // Initialize aHb - vpHomography H = aHb * v; - \endcode - -*/ vpHomography vpHomography::operator*(const double &v) const { vpHomography H; @@ -282,15 +191,6 @@ vpHomography vpHomography::operator*(const double &v) const return H; } -/*! - 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 b_P : 2D coordinates of the point in the image plane b. - - \return A point with 2D coordinates in the image plane a. -*/ vpPoint vpHomography::operator*(const vpPoint &b_P) const { vpPoint a_P; @@ -311,19 +211,7 @@ vpPoint vpHomography::operator*(const vpPoint &b_P) const return a_P; } -/*! - - Divide an homography by a scalar. - - \param v : Value of the scalar. - \code - vpHomography aHb; - // Initialize aHb - vpHomography H = aHb / aHb[2][2]; - \endcode - -*/ vpHomography vpHomography::operator/(const double &v) const { vpHomography H; @@ -339,7 +227,6 @@ vpHomography vpHomography::operator/(const double &v) const return H; } -//! Divide all the element of the homography matrix by v : Hij = Hij / v vpHomography &vpHomography::operator/=(double v) { // if (x == 0) @@ -354,12 +241,6 @@ vpHomography &vpHomography::operator/=(double v) return *this; } -/*! - Copy operator. - Allow operation such as aHb = H - - \param H : Homography matrix to be copied. -*/ vpHomography &vpHomography::operator=(const vpHomography &H) { for (unsigned int i = 0; i < 3; i++) @@ -370,12 +251,7 @@ vpHomography &vpHomography::operator=(const vpHomography &H) bP = H.bP; return *this; } -/*! - Copy operator. - Allow operation such as aHb = H - \param H : Matrix to be copied. -*/ vpHomography &vpHomography::operator=(const vpMatrix &H) { if (H.getRows() != 3 || H.getCols() != 3) @@ -388,14 +264,6 @@ vpHomography &vpHomography::operator=(const vpMatrix &H) return *this; } -/*! - Read an homography in a file, verify if it is really an homogeneous - matrix. - - \param f : the file. This file has to be written using save(). - - \sa save() -*/ void vpHomography::load(std::ifstream &f) { if (!f.fail()) { @@ -469,9 +337,6 @@ void vpHomography::build(vpHomography &aHb, const vpHomogeneousMatrix &aMb, cons aHb[i][j] = aHb_[i][j]; } -/*! - * Return homography determinant. - */ double vpHomography::det() const { return ((*this)[0][0] * ((*this)[1][1] * (*this)[2][2] - (*this)[1][2] * (*this)[2][1]) - @@ -479,10 +344,6 @@ double vpHomography::det() const (*this)[0][2] * ((*this)[1][0] * (*this)[2][1] - (*this)[1][1] * (*this)[2][0])); } -/*! - Set the homography as identity transformation by setting the diagonal to 1 - and all other values to 0. -*/ void vpHomography::eye() { for (unsigned int i = 0; i < 3; i++) @@ -503,18 +364,6 @@ void vpHomography::eye() void vpHomography::setIdentity() { eye(); } #endif // #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) -/*! - Given `iPa` a pixel with coordinates \f$(u_a,v_a)\f$ in - image a, and the homography `bHa` in the Euclidean space or calibrated domain that links image a and b, computes the - coordinates of the pixel \f$(u_b,v_b)\f$ in the image b using the camera - parameters matrix \f$\bf K\f$. - - Compute \f$^b{\bf p} = {\bf K} \; {^b}{\bf H}_a \; {\bf K}^{-1} {^a}{\bf - p}\f$ with \f$^a{\bf p}=(u_a,v_a,1)\f$ and \f$^b{\bf p}=(u_b,v_b,1)\f$ - - \return The coordinates in pixel of the point with coordinates - \f$(u_b,v_b)\f$. - */ vpImagePoint vpHomography::project(const vpCameraParameters &cam, const vpHomography &bHa, const vpImagePoint &iPa) { double xa = iPa.get_u(); @@ -529,18 +378,6 @@ vpImagePoint vpHomography::project(const vpCameraParameters &cam, const vpHomogr return iPb; } -/*! - Given `Pa` a point with normalized coordinates \f$(x_a,y_a,1)\f$ in the - image plane a, and the homography `bHa` in the Euclidean space that links image a and b, computes - the normalized coordinates of the point \f$(x_b,y_b,1)\f$ in the image plane - b. - - Compute \f$^b{\bf p} = {^b}{\bf H}_a \; {^a}{\bf p}\f$ with \f$^a{\bf - p}=(x_a,y_a,1)\f$ and \f$^b{\bf p}=(x_b,y_b,1)\f$ - - \return The coordinates in meter of the point with coordinates - \f$(x_b,y_b)\f$. - */ vpPoint vpHomography::project(const vpHomography &bHa, const vpPoint &Pa) { double xa = Pa.get_x(); @@ -556,39 +393,7 @@ vpPoint vpHomography::project(const vpHomography &bHa, const vpPoint &Pa) return Pb; } -/*! - From couples of matched points \f$^a{\bf p}=(x_a,y_a,1)\f$ in image a - and \f$^b{\bf p}=(x_b,y_b,1)\f$ in image b with homogeneous coordinates, - computes the homography matrix by resolving \f$^a{\bf p} = ^a{\bf H}_b\; - ^b{\bf p}\f$ using a robust estimation scheme. - - This method is to compare to DLT() except that here a robust estimator is - used to reject couples of points that are considered as outliers. - - At least 4 couples of points are needed. - - \param xb, yb : Coordinates vector of matched points in image b. These - coordinates are expressed in meters. - \param xa, ya : Coordinates vector of - matched points in image a. These coordinates are expressed in meters. - \param aHb : Estimated homography that relies the transformation from image a to - image b. - \param inliers : Vector that indicates if a matched point is an - inlier (true) or an outlier (false). - \param residual : Global residual computed as - \f$r = \sqrt{1/n \sum_{inliers} {\| {^a{\bf p} - {\hat{^a{\bf H}_b}} {^b{\bf p}}} \|}^{2}}\f$ - with \f$n\f$ the number of inliers. - \param weights_threshold : Threshold applied on the weights updated during the - robust estimation and used to consider if a point is an outlier or an - inlier. Values should be in [0:1]. A couple of matched points that have a - weight lower than this threshold is considered as an outlier. A value equal - to zero indicates that all the points are inliers. - \param niter : Number of iterations of the estimation process. - \param normalization : When set to true, the coordinates of the points are normalized. - The normalization carried out is the one preconized by Hartley. - - \sa DLT(), ransac() - */ + void vpHomography::robust(const std::vector &xb, const std::vector &yb, const std::vector &xa, const std::vector &ya, vpHomography &aHb, std::vector &inliers, double &residual, double weights_threshold, unsigned int niter, bool normalization) @@ -725,13 +530,6 @@ void vpHomography::robust(const std::vector &xb, const std::vectorh is the column of V associated with the smalest singular - value of A - - - \param xb, yb : Coordinates vector of matched points in image b. These - coordinates are expressed in meters. \param xa, ya : Coordinates vector of - matched points in image a. These coordinates are expressed in meters. \param - aHb : Estimated homography that relies the transformation from image a to - image b. \param normalization : When set to true, the coordinates of the - points are normalized. The normalization carried out is the one preconized - by Hartley. - - \exception vpMatrixException::rankDeficient : When the rank of the matrix - that should be 8 is deficient. -*/ void vpHomography::DLT(const std::vector &xb, const std::vector &yb, const std::vector &xa, const std::vector &ya, vpHomography &aHb, bool normalization) { diff --git a/modules/vision/src/homography-estimation/vpHomographyExtract.cpp b/modules/vision/src/homography-estimation/vpHomographyExtract.cpp index d577f02c89..fb74eb639a 100644 --- a/modules/vision/src/homography-estimation/vpHomographyExtract.cpp +++ b/modules/vision/src/homography-estimation/vpHomographyExtract.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: * Homography transformation. - * -*****************************************************************************/ + */ #include #include @@ -44,17 +42,6 @@ /* ------------------------------------------------------------------------ */ const double vpHomography::sing_threshold = 0.0001; -/*! - Compute the camera displacement between two images from the homography \f$ - {^a}{\bf H}_b \f$ which is here an implicit parameter (*this). - - \param aRb : Rotation matrix as an output \f$ {^a}{\bf R}_b \f$. - - \param atb : Translation vector as an output \f$ ^a{\bf t}_b \f$. - - \param n : Normal vector to the plane as an output. - -*/ void vpHomography::computeDisplacement(vpRotationMatrix &aRb, vpTranslationVector &atb, vpColVector &n) { @@ -66,26 +53,6 @@ void vpHomography::computeDisplacement(vpRotationMatrix &aRb, vpTranslationVecto computeDisplacement(*this, aRb, atb, n); } -/*! - - Compute the camera displacement between two images from the homography \f$ - {^a}{\bf H}_b \f$ which is here an implicit parameter (*this). - - Camera displacement between \f$ {^a}{\bf p} \f$ and \f$ {^a}{\bf p} \f$ is - represented as a rotation matrix \f$ {^a}{\bf R}_b \f$ and a translation - vector \f$ ^a{\bf t}_b \f$ from which an homogeneous matrix can be build - (vpHomogeneousMatrix). - - \param nd : Input normal vector to the plane used to compar with the normal - vector \e n extracted from the homography. - - \param aRb : Rotation matrix as an output \f$ {^a}{\bf R}_b \f$. - - \param atb : Translation vector as an output \f$ ^a{\bf t}_b \f$. - - \param n : Normal vector to the plane as an output. - -*/ void vpHomography::computeDisplacement(const vpColVector &nd, vpRotationMatrix &aRb, vpTranslationVector &atb, vpColVector &n) { diff --git a/modules/vision/src/homography-estimation/vpHomographyMalis.cpp b/modules/vision/src/homography-estimation/vpHomographyMalis.cpp index 3da1e06b3a..5fde2674a6 100644 --- a/modules/vision/src/homography-estimation/vpHomographyMalis.cpp +++ b/modules/vision/src/homography-estimation/vpHomographyMalis.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: * Homography estimation. - * -*****************************************************************************/ + */ /*! \file vpHomographyMalis.cpp @@ -63,21 +61,21 @@ const double eps = 1e-6; * **************************************************************************** * ENTREES : - * int pts_ref[4] : Definit quels sont les points de reference, ils ne - * seront pas affectes par le changement de repere - * int nb_pts : nombre de points a changer de repere - * double **pd : La matrice des coordonnees des points desires - * double **p : La matrice des coordonnees des points courants + * int pts_ref[4] : Definit quels sont les points de reference, ils ne + * seront pas affectes par le changement de repere + * int nb_pts : nombre de points a changer de repere + * double **pd : La matrice des coordonnees des points desires + * double **p : La matrice des coordonnees des points courants * * * SORTIES : * - * double **pt_des_nr : La matrice des coordonnees des points desires - * dans le nouveau repere. - * double **pt_cour_nr : La matrice des coordonnees des points courants - * dans le nouveau repere - * double **M : ?? - * double **Mpd : pseudo inverse de M .. + * double **pt_des_nr : La matrice des coordonnees des points desires + * dans le nouveau repere. + * double **pt_cour_nr : La matrice des coordonnees des points courants + * dans le nouveau repere + * double **M : ?? + * double **Mpd : pseudo inverse de M .. * * **************************************************************************** @@ -161,13 +159,13 @@ void changeFrame(unsigned int *pts_ref, unsigned int nb_pts, vpMatrix &pd, vpMat * **************************************************************************** * ENTREES : - * int Nb_pts : nombre de points - * double **pd : tableau des coordonnees des points desires - * couble **p : tableau des coordonnees des points courants + * int Nb_pts : nombre de points + * double **pd : tableau des coordonnees des points desires + * couble **p : tableau des coordonnees des points courants * * SORTIES : * - * double **H matrice d homographie + * double **H matrice d homographie * **************************************************************************** * AUTEUR : BOSSARD Nicolas. INSA Rennes 5eme annee. @@ -225,7 +223,7 @@ void HLM2D(unsigned int nb_pts, vpMatrix &points_des, vpMatrix &points_cour, vpM /***** La meilleure solution est le vecteur de V associe - a la valeur singuliere la plus petite en valeur absolu. + a la valeur singuliere la plus petite en valeur absolu. Pour cela on parcourt la matrice des valeurs singulieres et on repere la plus petite valeur singuliere, on en profite pour effectuer un controle sur le rang de la matrice : pas plus @@ -274,14 +272,14 @@ void HLM2D(unsigned int nb_pts, vpMatrix &points_des, vpMatrix &points_cour, vpM * **************************************************************************** * ENTREES : - * int Nb_pts : nombre de points - * double **pd : tableau des coordonnees des points desires - * couble **p : tableau des coordonnees des points courants + * int Nb_pts : nombre de points + * double **pd : tableau des coordonnees des points desires + * couble **p : tableau des coordonnees des points courants * * SORTIES : * - * double **H matrice d'homographie - * double epipole[3] epipole + * double **H matrice d'homographie + * double epipole[3] epipole * **************************************************************************** **/ @@ -476,14 +474,14 @@ void HLM3D(unsigned int nb_pts, vpMatrix &pd, vpMatrix &p, vpMatrix &H) // estimation de a = 1,b,c ; je cherche le min de somme(i=1:n) // (0.5*(ei)^2) - // e1 = V[1][.] * b - V[3][.] = 0 ; - // e2 = V[2][.] * c - V[3][.] = 0 ; - // e3 = V[2][.] * b - V[3][.] * c = 0 ; - // e4 = V[4][.] * b - V[5][.] = 0 ; - // e5 = V[4][.] * c - V[6][.] = 0 ; - // e6 = V[6][.] * b - V[5][.] * c = 0 ; - // e7 = V[7][.] * b - V[8][.] = 0 ; - // e8 = V[7][.] * c - V[9][.] = 0 ; + // e1 = V[1][.] * b - V[3][.] = 0 ; + // e2 = V[2][.] * c - V[3][.] = 0 ; + // e3 = V[2][.] * b - V[3][.] * c = 0 ; + // e4 = V[4][.] * b - V[5][.] = 0 ; + // e5 = V[4][.] * c - V[6][.] = 0 ; + // e6 = V[6][.] * b - V[5][.] * c = 0 ; + // e7 = V[7][.] * b - V[8][.] = 0 ; + // e8 = V[7][.] * c - V[9][.] = 0 ; d[0] = V[2][vect]; d[1] = V[4][vect]; d[2] = V[1][vect]; @@ -594,28 +592,6 @@ void HLM(unsigned int q_cible, const std::vector &xm, const std::vector< #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS -/*! - From couples of matched points \f$^a{\bf p}=(x_a,y_a,1)\f$ in image a - and \f$^b{\bf p}=(x_b,y_b,1)\f$ in image b with homogeneous coordinates, - computes the homography matrix by resolving \f$^a{\bf p} = ^a{\bf H}_b\; - ^b{\bf p}\f$ using Ezio Malis linear method (HLM) \cite Malis00b. - - This method can consider points that are planar or non planar. The algorithm - for planar scene implemented in this file is described in Ezio Malis PhD - thesis \cite TheseMalis. - - \param xb, yb : Coordinates vector of matched points in image b. These - coordinates are expressed in meters. \param xa, ya : Coordinates vector of - matched points in image a. These coordinates are expressed in meters. \param - isplanar : If true the points are assumed to be in a plane, otherwise there - are assumed to be non planar. \param aHb : Estimated homography that relies - the transformation from image a to image b. - - If the boolean isplanar is true the points are assumed to be in a plane - otherwise there are assumed to be non planar. - - \sa DLT() when the scene is planar. -*/ void vpHomography::HLM(const std::vector &xb, const std::vector &yb, const std::vector &xa, const std::vector &ya, bool isplanar, vpHomography &aHb) { diff --git a/modules/vision/src/homography-estimation/vpHomographyRansac.cpp b/modules/vision/src/homography-estimation/vpHomographyRansac.cpp index 76cd148e95..891e494cf2 100644 --- a/modules/vision/src/homography-estimation/vpHomographyRansac.cpp +++ b/modules/vision/src/homography-estimation/vpHomographyRansac.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: * Homography estimation. - * -*****************************************************************************/ + */ #include #include @@ -283,35 +281,6 @@ void vpHomography::initRansac(unsigned int n, double *xb, double *yb, double *xa } } -/*! - - From couples of matched points \f$^a{\bf p}=(x_a,y_a,1)\f$ in image a - and \f$^b{\bf p}=(x_b,y_b,1)\f$ in image b with homogeneous coordinates, - computes the homography matrix by resolving \f$^a{\bf p} = ^a{\bf H}_b\; - ^b{\bf p}\f$ using Ransac algorithm. - - \param xb, yb : Coordinates vector of matched points in image b. These - coordinates are expressed in meters. \param xa, ya : Coordinates vector of - matched points in image a. These coordinates are expressed in meters. \param - aHb : Estimated homography that relies the transformation from image a to - image b. \param inliers : Vector that indicates if a matched point is an - inlier (true) or an outlier (false). \param residual : Global residual - computed as \f$r = \sqrt{1/n \sum_{inliers} {\| {^a{\bf p} - {\hat{^a{\bf - H}_b}} {^b{\bf p}}} \|}^{2}}\f$ with \f$n\f$ the number of inliers. - - \param nbInliersConsensus : Minimal number of points requested to fit the - estimated homography. - - \param threshold : Threshold for outlier removing. A point is considered as - an outlier if the reprojection error \f$\| {^a{\bf p} - {\hat{^a{\bf H}_b}} - {^b{\bf p}}} \|\f$ is greater than this threshold. - - \param normalization : When set to true, the coordinates of the points are - normalized. The normalization carried out is the one preconized by Hartley. - - \return true if the homography could be computed, false otherwise. - -*/ bool vpHomography::ransac(const std::vector &xb, const std::vector &yb, const std::vector &xa, const std::vector &ya, vpHomography &aHb, std::vector &inliers, double &residual, unsigned int nbInliersConsensus, double threshold, bool normalization) diff --git a/modules/vision/src/homography-estimation/vpHomographyVVS.cpp b/modules/vision/src/homography-estimation/vpHomographyVVS.cpp index 296ba7b4b5..76a29c76eb 100644 --- a/modules/vision/src/homography-estimation/vpHomographyVVS.cpp +++ b/modules/vision/src/homography-estimation/vpHomographyVVS.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: * Homography transformation. - * -*****************************************************************************/ + */ //#include //#include diff --git a/modules/vision/src/key-point/vpBasicKeyPoint.cpp b/modules/vision/src/key-point/vpBasicKeyPoint.cpp deleted file mode 100644 index 1abd5ee1f4..0000000000 --- a/modules/vision/src/key-point/vpBasicKeyPoint.cpp +++ /dev/null @@ -1,44 +0,0 @@ -/**************************************************************************** - * - * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. - * - * This software is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * See the file LICENSE.txt at the root directory of this source - * distribution for additional information about the GNU GPL. - * - * For using ViSP with software that can not be combined with the GNU - * GPL, please contact Inria about acquiring a ViSP Professional - * Edition License. - * - * See https://visp.inria.fr for more information. - * - * This software was developed at: - * Inria Rennes - Bretagne Atlantique - * Campus Universitaire de Beaulieu - * 35042 Rennes Cedex - * France - * - * If you have questions regarding the use of this file, please contact - * Inria at visp@inria.fr - * - * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE - * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - * - * Description: - * Key point used in matching algorithm. - * -*****************************************************************************/ - -#include - -/*! - Basic constructor. -*/ -vpBasicKeyPoint::vpBasicKeyPoint() - : referenceImagePointsList(), currentImagePointsList(), matchedReferencePoints(), _reference_computed(false) -{ -} diff --git a/modules/vision/src/key-point/vpFernClassifier.cpp b/modules/vision/src/key-point/vpFernClassifier.cpp index c8740b4636..f78f0b87ab 100644 --- a/modules/vision/src/key-point/vpFernClassifier.cpp +++ b/modules/vision/src/key-point/vpFernClassifier.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -31,8 +30,7 @@ * Description: * Class that implements the Fern classifier and the YAPE detector thanks * to the OpenCV library. - * -*****************************************************************************/ + */ #include diff --git a/modules/vision/src/key-point/vpKeyPoint.cpp b/modules/vision/src/key-point/vpKeyPoint.cpp index 854fb3117b..ee548b5e46 100644 --- a/modules/vision/src/key-point/vpKeyPoint.cpp +++ b/modules/vision/src/key-point/vpKeyPoint.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: * Key point functionalities. - * -*****************************************************************************/ + */ #include #include diff --git a/modules/vision/src/key-point/vpPlanarObjectDetector.cpp b/modules/vision/src/key-point/vpPlanarObjectDetector.cpp index e7878df979..d0c19e5b4a 100644 --- a/modules/vision/src/key-point/vpPlanarObjectDetector.cpp +++ b/modules/vision/src/key-point/vpPlanarObjectDetector.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: * Planar surface detection tool. - * -*****************************************************************************/ + */ #include diff --git a/modules/vision/src/key-point/vpXmlConfigParserKeyPoint.cpp b/modules/vision/src/key-point/vpXmlConfigParserKeyPoint.cpp index 2ba8e21771..0d298769c5 100644 --- a/modules/vision/src/key-point/vpXmlConfigParserKeyPoint.cpp +++ b/modules/vision/src/key-point/vpXmlConfigParserKeyPoint.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * XML parser to load configuration for vpKeyPoint class. - * - * Authors: - * Souriya Trinh - * -*****************************************************************************/ + */ /*! \file vpXmlConfigParserKeyPoint.cpp diff --git a/modules/vision/src/plane-estimation/vpPlaneEstimation.cpp b/modules/vision/src/plane-estimation/vpPlaneEstimation.cpp index 45727a6108..39122eacd0 100644 --- a/modules/vision/src/plane-estimation/vpPlaneEstimation.cpp +++ b/modules/vision/src/plane-estimation/vpPlaneEstimation.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Class for Plane Estimation. - * - * Authors: - * Julien Dufour - * -*****************************************************************************/ + */ #include diff --git a/modules/vision/src/pose-estimation/vpLevenbergMarquartd.cpp b/modules/vision/src/pose-estimation/vpLevenbergMarquartd.cpp index 90958c7bb0..677f57d808 100644 --- a/modules/vision/src/pose-estimation/vpLevenbergMarquartd.cpp +++ b/modules/vision/src/pose-estimation/vpLevenbergMarquartd.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Levenberg Marquartd. - * - * Authors: - * Francois Chaumette - * -*****************************************************************************/ + */ #include // (std::min) #include // std::fabs @@ -56,14 +51,14 @@ #define FALSE 0 /* - * PROCEDURE : enorm + * PROCEDURE : enorm * - * ENTREE : + * ENTREE : * - * x Vecteur de taille "n" - * n Taille du vecteur "x" + * x Vecteur de taille "n" + * n Taille du vecteur "x" * - * DESCRIPTION : + * 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 @@ -76,7 +71,7 @@ * pas en overflow. Les constantes donnees ici conviennent pour la plupart des * pc connus. * - * RETOUR : + * 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. @@ -100,14 +95,14 @@ double enorm(const double *x, int n) double xabs = std::fabs(x[i]); if ((xabs > rdwarf) && (xabs < agiant)) { /* - * somme pour elements intemediaires. + * somme pour elements intemediaires. */ s2 += xabs * xabs; } else if (xabs <= rdwarf) { /* - * somme pour elements petits. + * somme pour elements petits. */ if (xabs <= x3max) { // if (xabs != 0.0) @@ -121,7 +116,7 @@ double enorm(const double *x, int n) else { /* - * somme pour elements grand. + * somme pour elements grand. */ if (xabs <= x1max) { s1 += (xabs / x1max) * (xabs / x1max); @@ -133,7 +128,7 @@ double enorm(const double *x, int n) } /* - * calcul de la norme. + * calcul de la norme. */ // if (s1 == 0.0) if (std::fabs(s1) <= std::numeric_limits::epsilon()) { @@ -150,39 +145,39 @@ double enorm(const double *x, int n) return (norm_eucl); } -/* PROCEDURE : lmpar +/* PROCEDURE : lmpar * - * ENTREE : - * n Ordre de la matrice "r". - * r Matrice de taille "n" x "n". En entree, la toute la partie - * triangulaire superieure doit contenir toute la partie + * ENTREE : + * n Ordre de la matrice "r". + * r Matrice de taille "n" x "n". En entree, la toute la partie + * triangulaire superieure doit contenir toute la partie *triangulaire superieure de "r". * - * ldr Taille maximum de la matrice "r". "ldr" >= "n". + * ldr Taille maximum de la matrice "r". "ldr" >= "n". * - * ipvt Vecteur de taille "n" qui definit la matrice de permutation + * ipvt Vecteur de taille "n" qui definit la matrice de permutation *"p" tel que : a * p = q * r. La jeme colonne de p la colonne ipvt[j] de la *matrice d'identite. * - * diag Vecteur de taille "n" contenant les elements diagonaux de la - * matrice "d". + * diag Vecteur de taille "n" contenant les elements diagonaux de la + * matrice "d". * - * qtb Vecteur de taille "n" contenant les "n" premiers elements du - * vecteur (q transpose)*b. + * qtb Vecteur de taille "n" contenant les "n" premiers elements du + * vecteur (q transpose)*b. * - * delta Limite superieure de la norme euclidienne de d * x. + * delta Limite superieure de la norme euclidienne de d * x. * - * par Estimee initiale du parametre de Levenberg-Marquardt. - * wa1, wa2 Vecteurs de taille "n" de travail. + * par Estimee initiale du parametre de Levenberg-Marquardt. + * wa1, wa2 Vecteurs de taille "n" de travail. * - * DESCRIPTION : + * DESCRIPTION : * La procedure determine le parametre de Levenberg-Marquardt. Soit une *matrice "a" de taille "m" x "n", une matrice diagonale "d" non singuliere de *taille "n" x "n", un vecteur "b" de taille "m" et un nombre positf delta, *le probleme est le calcul du parametre "par" de telle facon que si "x" *resoud le systeme * - * a * x = b , sqrt(par) * d * x = 0 , + * a * x = b , sqrt(par) * d * x = 0 , * * au sens des moindre carre, et dxnorm est la norme euclidienne de d * x * alors "par" vaut 0.0 et (dxnorm - delta) <= 0.1 * delta , @@ -206,38 +201,38 @@ double enorm(const double *x, int n) * l'algorithme. Si neanmoins la limite de 10 iterations est atteinte, la * valeur de sortie "par" aura la derniere meilleure valeur. * - * SORTIE : - * r En sortie, tout le triangle superieur est inchange, et le - * le triangle inferieur contient les elements de la partie - * triangulaire superieure (transpose) de la matrice triangulaire - * superieure de "s". - * par Estimee finale du parametre de Levenberg-Marquardt. - * x Vecteur de taille "n" contenant la solution au sens des + * SORTIE : + * r En sortie, tout le triangle superieur est inchange, et le + * le triangle inferieur contient les elements de la partie + * triangulaire superieure (transpose) de la matrice triangulaire + * superieure de "s". + * par Estimee finale du parametre de Levenberg-Marquardt. + * x Vecteur de taille "n" contenant la solution au sens des *moindres carres du systeme a * x = b, sqrt(par) * d * x = 0, pour le - * parametre en sortie "par" - * sdiag Vecteur de taille "n" contenant les elements diagonaux de la - * matrice triangulaire "s". + * parametre en sortie "par" + * sdiag Vecteur de taille "n" contenant les elements diagonaux de la + * matrice triangulaire "s". * - * RETOUR : + * RETOUR : * En cas de succes, la valeur 0.0 est retournee. * */ int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, double *delta, double *par, double *x, double *sdiag, double *wa1, double *wa2) { - const double tol1 = 0.1; /* tolerance a 0.1 */ + const double tol1 = 0.1; /* tolerance a 0.1 */ int l; unsigned int iter; /* compteur d'iteration */ int nsing; /* nombre de singularite de la matrice */ double dxnorm, fp; double temp; - double dwarf = DBL_MIN; /* plus petite amplitude positive */ + double dwarf = DBL_MIN; /* plus petite amplitude positive */ /* - * calcul et stockage dans "x" de la direction de Gauss-Newton. Si - * le jacobien a une rangee de moins, on a une solution au moindre - * carres. + * calcul et stockage dans "x" de la direction de Gauss-Newton. Si + * le jacobien a une rangee de moins, on a une solution au moindre + * carres. */ nsing = n; @@ -272,9 +267,9 @@ int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doubl } /* - * initialisation du compteur d'iteration. - * evaluation de la fonction a l'origine, et test - * d'acceptation de la direction de Gauss-Newton. + * initialisation du compteur d'iteration. + * evaluation de la fonction a l'origine, et test + * d'acceptation de la direction de Gauss-Newton. */ iter = 0; @@ -287,9 +282,9 @@ int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doubl if (fp > tol1 * (*delta)) { /* - * Si le jacobien n'a pas de rangee deficiente,l'etape de - * Newton fournit une limite inferieure, parl pour le - * zero de la fonction. Sinon cette limite vaut 0.0. + * Si le jacobien n'a pas de rangee deficiente,l'etape de + * Newton fournit une limite inferieure, parl pour le + * zero de la fonction. Sinon cette limite vaut 0.0. */ double parl = 0.0; @@ -316,8 +311,8 @@ int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doubl } /* - * calcul d'une limite superieure, paru, pour le zero de la - * fonction. + * calcul d'une limite superieure, paru, pour le zero de la + * fonction. */ for (int i = 0; i < n; i++) { double sum = 0.0; @@ -337,8 +332,8 @@ int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doubl paru = dwarf / vpMath::minimum(*delta, tol1); /* - * Si "par" en entree tombe hors de l'intervalle (parl,paru), - * on le prend proche du point final. + * Si "par" en entree tombe hors de l'intervalle (parl,paru), + * on le prend proche du point final. */ *par = vpMath::maximum(*par, parl); @@ -349,19 +344,19 @@ int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doubl *par = gnorm / dxnorm; /* - * debut d'une iteration. + * debut d'une iteration. */ for (;;) // iter >= 0) { iter++; /* - * evaluation de la fonction a la valeur courant - * de "par". + * evaluation de la fonction a la valeur courant + * de "par". */ // if (*par == 0.0) if (std::fabs(*par) <= std::numeric_limits::epsilon()) { - const double tol001 = 0.001; /* tolerance a 0.001 */ + const double tol001 = 0.001; /* tolerance a 0.001 */ *par = vpMath::maximum(dwarf, (tol001 * paru)); } @@ -380,14 +375,14 @@ int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doubl fp = dxnorm - *delta; /* - * si la fonction est assez petite, acceptation de - * la valeur courant de "par". de plus, test des cas - * ou parl est nul et ou le nombre d'iteration a - * atteint 10. + * si la fonction est assez petite, acceptation de + * la valeur courant de "par". de plus, test des cas + * ou parl est nul et ou le nombre d'iteration a + * atteint 10. */ // if ((std::fabs(fp) <= tol1 * (*delta)) || ((parl == 0.0) && (fp <= // temp) - // && (temp < 0.0)) || (iter == 10)) + // && (temp < 0.0)) || (iter == 10)) if ((std::fabs(fp) <= tol1 * (*delta)) || ((std::fabs(parl) <= std::numeric_limits::epsilon()) && (fp <= temp) && (temp < 0.0)) || (iter == 10)) { @@ -423,8 +418,8 @@ int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doubl double parc = ((fp / *delta) / temp) / temp; /* - * selon le signe de la fonction, mise a jour - * de parl ou paru. + * selon le signe de la fonction, mise a jour + * de parl ou paru. */ if (fp > 0.0) parl = vpMath::maximum(parl, *par); @@ -433,14 +428,14 @@ int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doubl paru = vpMath::minimum(paru, *par); /* - * calcul d'une estimee ameliree de "par". + * calcul d'une estimee ameliree de "par". */ *par = vpMath::maximum(parl, (*par + parc)); - } /* fin boucle sur iter */ - } /* fin fp > tol1 * delta */ + } /* fin boucle sur iter */ + } /* fin fp > tol1 * delta */ /* - * terminaison. + * terminaison. */ if (iter == 0) *par = 0.0; @@ -449,16 +444,16 @@ int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doubl } /* - * PROCEDURE : pythag + * PROCEDURE : pythag * - * ENTREES : - * a, b Variables dont on veut la racine carre de leur somme de carre + * ENTREES : + * a, b Variables dont on veut la racine carre de leur somme de carre * - * DESCRIPTION : + * DESCRIPTION : * La procedure calcule la racine carre de la somme des carres de deux nombres * en evitant l'overflow ou l'underflow destructif. * - * RETOUR : + * RETOUR : * La procedure retourne la racine carre de a^2 + b^2. * */ @@ -491,22 +486,22 @@ double pythag(double a, double b) } /* - * PROCEDURE : qrfac + * PROCEDURE : qrfac * - * ENTREE : - * m Nombre de lignes de la matrice "a". - * n Nombre de colonne de la matrice "a". - * a Matrice de taille "m" x "n". elle contient, en entree la + * ENTREE : + * m Nombre de lignes de la matrice "a". + * n Nombre de colonne de la matrice "a". + * a Matrice de taille "m" x "n". elle contient, en entree la *matrice dont on veut sa factorisation qr. - * lda Taille maximale de "a". lda >= m. - * pivot Booleen. Si pivot est TRUE, le pivotage de colonnes est + * lda Taille maximale de "a". lda >= m. + * pivot Booleen. Si pivot est TRUE, le pivotage de colonnes est *realise Si pivot = FALSE, pas de pivotage. - * lipvt Taille du vecteur "ipvt". Si pivot est FALSE, lipvt est de - * l'ordre de 1. Sinon lipvt est de l'ordre de "n". - * wa Vecteur de travail de taille "n". Si pivot = FALSE "wa" - * coincide avec rdiag. + * lipvt Taille du vecteur "ipvt". Si pivot est FALSE, lipvt est de + * l'ordre de 1. Sinon lipvt est de l'ordre de "n". + * wa Vecteur de travail de taille "n". Si pivot = FALSE "wa" + * coincide avec rdiag. * - * DESCRIPTION : + * DESCRIPTION : * La procedure effectue une decomposition de la matrice "a"par la methode qr. * Elle utilise les transformations de householders avec pivotage sur les *colonnes (option) pour calculer la factorisation qr de la matrice "a" de @@ -516,23 +511,23 @@ double pythag(double a, double b) *valeurs,tel que a * p = q * r. La transformation de householder pour la *colonne k, k = 1,2,...,min(m,n), est de la forme * t - * i - (1 / u(k)) * u * u + * i - (1 / u(k)) * u * u * * Ou u a des zeros dans les k-1 premieres positions. * - * SORTIE : - * a Matrice de taille "m" x "n" dont le trapeze superieur de "a" - * contient la partie trapezoidale superieure de "r" et la partie - * trapezoidale inferieure de "a" contient une forme factorisee - * de "q" (les elements non triviaux du vecteurs "u" sont decrits - * ci-dessus). - * ipvt Vecteur de taille "n". Il definit la matrice de permutation + * SORTIE : + * a Matrice de taille "m" x "n" dont le trapeze superieur de "a" + * contient la partie trapezoidale superieure de "r" et la partie + * trapezoidale inferieure de "a" contient une forme factorisee + * de "q" (les elements non triviaux du vecteurs "u" sont decrits + * ci-dessus). + * ipvt Vecteur de taille "n". Il definit la matrice de permutation *"p" tel que a * p = q * r. La jeme colonne de p est la colonne ipvt[j] de la *matrice d'identite. Si pivot = FALSE, ipvt n'est pas referencee. rdiag *Vecteur de taille "n" contenant les elements diagonaux de la matrice - *"r". acnorm Vecteur de taille "n" contenant les normes des lignes - * correspondantes de la matrice "a". Si cette information n'est - * pas requise, acnorm coincide avec rdiag. + *"r". acnorm Vecteur de taille "n" contenant les normes des lignes + * correspondantes de la matrice "a". Si cette information n'est + * pas requise, acnorm coincide avec rdiag. * */ int qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt, int /* lipvt */, double *rdiag, double *acnorm, @@ -545,13 +540,13 @@ int qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt, int /* lipvt double sum, temp, tmp; /* - * epsmch est la precision machine. + * epsmch est la precision machine. */ epsmch = std::numeric_limits::epsilon(); /* - * calcul des normes initiales des lignes et initialisation - * de plusieurs tableaux. + * calcul des normes initiales des lignes et initialisation + * de plusieurs tableaux. */ for (i = 0; i < m; i++) { acnorm[i] = enorm(MIJ(a, i, 0, lda), n); @@ -568,8 +563,8 @@ int qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt, int /* lipvt for (i = 0; i < minmn; i++) { if (pivot) { /* - * met la ligne de plus grande norme en position - * de pivot. + * met la ligne de plus grande norme en position + * de pivot. */ kmax = i; for (k = i; k < m; k++) { @@ -589,8 +584,8 @@ int qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt, int /* lipvt } /* - * calcul de al transformationde Householder afin de reduire - * la jeme ligne de "a" a un multiple du jeme vecteur unite. + * calcul de al transformationde Householder afin de reduire + * la jeme ligne de "a" a un multiple du jeme vecteur unite. */ double ajnorm = enorm(MIJ(a, i, i, lda), n - i); @@ -604,8 +599,8 @@ int qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt, int /* lipvt *MIJ(a, i, i, lda) += 1.0; /* - * application de la tranformation aux lignes - * restantes et mise a jour des normes. + * application de la tranformation aux lignes + * restantes et mise a jour des normes. */ ip1 = i + 1; @@ -630,7 +625,7 @@ int qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt, int /* lipvt wa[k] = rdiag[k]; } } - } /* fin boucle for k */ + } /* fin boucle for k */ } } /* fin if (ajnorm) */ @@ -640,30 +635,30 @@ int qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt, int /* lipvt return (0); } -/* PROCEDURE : qrsolv +/* PROCEDURE : qrsolv * - * ENTREE : - * n Ordre de la matrice "r". - * r Matrice de taille "n" x "n". En entree, la partie triangulaire - * complete de "r" doit contenir la partie triangulaire + * ENTREE : + * n Ordre de la matrice "r". + * r Matrice de taille "n" x "n". En entree, la partie triangulaire + * complete de "r" doit contenir la partie triangulaire *superieure complete de "r". - * ldr Taille maximale de la matrice "r". "ldr" >= n. - * ipvt Vecteur de taille "n" definissant la matrice de permutation + * ldr Taille maximale de la matrice "r". "ldr" >= n. + * ipvt Vecteur de taille "n" definissant la matrice de permutation *"p" La jeme colonne de de "p" est la colonne ipvt[j] de la matrice identite. - * diag Vecteur de taille "n" contenant les elements diagonaux de la - * matrice "d". - * qtb Vecteur de taille "n" contenant les "n" premiers elements du - * vecteur (q transpose) * b. - * wa Vecteur de travail de taille "n". + * diag Vecteur de taille "n" contenant les elements diagonaux de la + * matrice "d". + * qtb Vecteur de taille "n" contenant les "n" premiers elements du + * vecteur (q transpose) * b. + * wa Vecteur de travail de taille "n". * - * DESCRIPTION : + * DESCRIPTION : * La procedure complete la solution du probleme, si on fournit les *information necessaires de la factorisation qr, avec pivotage des colonnes. * Soit une matrice "a" de taille "m" x "n" donnee, une matrice diagonale "d" *de taille "n" x "n" et un vecteur "b" de taille "m", le probleme est la *determination un vecteur "x" qui est solution du systeme * - * a*x = b , d*x = 0 , + * a*x = b , d*x = 0 , * * Au sens des moindres carres. * @@ -674,7 +669,7 @@ int qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt, int /* lipvt *la matrice de permutaion "p" et les "n" premiers elements de (q transpose) ** b. Le systeme * - * a * x = b, d * x = 0, est alors equivalent a + * a * x = b, d * x = 0, est alors equivalent a * * t t * r * z = q * b , p * d * p * z = 0 , @@ -688,25 +683,25 @@ int qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt, int /* lipvt * * "s" est calculee a l'interieure de qrsolv et peut etre hors interet. * - * SORTIE : - * r En sortie, le triangle superieur n'est pas altere, et la + * SORTIE : + * r En sortie, le triangle superieur n'est pas altere, et la *partie triangulaire inferieure contient la partie triangulaire superieure - * (transpose) de la matrice triangulaire "s". - * x Vecteur de taille "n" contenant les solutions au moindres - *carres du systeme a * x = b, d * x = 0. sdiag Vecteur de taille "n" + * (transpose) de la matrice triangulaire "s". + * x Vecteur de taille "n" contenant les solutions au moindres + *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 qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, double *x, double *sdiag, double *wa) { - int i, j, k, kp1, l; /* compteur de boucle */ + int i, j, k, kp1, l; /* compteur de boucle */ int nsing; double cosi, cotg, qtbpj, sinu, tg, temp; /* - * copie de r et (q transpose) * b afin de preserver l'entree - * et initialisation de "s". En particulier, sauvegarde des elements - * diagonaux de "r" dans "x". + * copie de r et (q transpose) * b afin de preserver l'entree + * et initialisation de "s". En particulier, sauvegarde des elements + * diagonaux de "r" dans "x". */ for (i = 0; i < n; i++) { for (j = i; j < n; j++) @@ -717,15 +712,15 @@ int qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doub } /* - * Elimination de la matrice diagonale "d" en utlisant une rotation - * connue. + * Elimination de la matrice diagonale "d" en utlisant une rotation + * connue. */ for (i = 0; i < n; i++) { /* - * preparation de la colonne de d a eliminer, reperage de - * l'element diagonal par utilisation de p de la - * factorisation qr. + * preparation de la colonne de d a eliminer, reperage de + * l'element diagonal par utilisation de p de la + * factorisation qr. */ l = ipvt[i]; @@ -737,19 +732,19 @@ int qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doub sdiag[i] = diag[l]; /* - * Les transformations qui eliminent la colonne de d - * modifient seulement qu'un seul element de - * (q transpose)*b avant les n premiers elements - * lesquels sont inialement nuls. + * Les transformations qui eliminent la colonne de d + * modifient seulement qu'un seul element de + * (q transpose)*b avant les n premiers elements + * lesquels sont inialement nuls. */ qtbpj = 0.0; for (k = i; k < n; k++) { /* - * determination d'une rotation qui elimine - * les elements appropriees dans la colonne - * courante de d. + * determination d'une rotation qui elimine + * les elements appropriees dans la colonne + * courante de d. */ // if (sdiag[k] != 0.0) @@ -765,9 +760,9 @@ int qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doub } /* - * calcul des elements de la diagonale modifiee - * de r et des elements modifies de - * ((q transpose)*b,0). + * calcul des elements de la diagonale modifiee + * de r et des elements modifies de + * ((q transpose)*b,0). */ *MIJ(r, k, k, ldr) = cosi * *MIJ(r, k, k, ldr) + sinu * sdiag[k]; temp = cosi * wa[k] + sinu * qtbpj; @@ -775,8 +770,8 @@ int qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doub wa[k] = temp; /* - * accumulation des tranformations dans - * les lignes de s. + * accumulation des tranformations dans + * les lignes de s. */ kp1 = k + 1; @@ -788,21 +783,21 @@ int qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doub *MIJ(r, k, j, ldr) = temp; } } - } /* fin if diag[] !=0 */ + } /* fin if diag[] !=0 */ } /* fin boucle for k -> n */ - } /* fin if diag =0 */ + } /* fin if diag =0 */ /* - * stokage de l'element diagonal de s et restauration de - * l'element diagonal correspondant de r. + * stokage de l'element diagonal de s et restauration de + * l'element diagonal correspondant de r. */ sdiag[i] = *MIJ(r, i, i, ldr); *MIJ(r, i, i, ldr) = x[i]; - } /* fin boucle for j -> n */ + } /* fin boucle for j -> n */ /* - * resolution du systeme triangulaire pour z. Si le systeme est - * singulier, on obtient une solution au moindres carres. + * resolution du systeme triangulaire pour z. Si le systeme est + * singulier, on obtient une solution au moindres carres. */ nsing = n; @@ -829,7 +824,7 @@ int qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doub } } /* - * permutation arriere des composants de z et des componants de x. + * permutation arriere des composants de z et des componants de x. */ for (j = 0; j < n; j++) { @@ -843,31 +838,31 @@ int qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doub * PROCEDURE : lmder * * - * ENTREE : - * fcn Fonction qui calcule la fonction et le jacobien de la - *fonction. m Nombre de fonctions. - * n Nombre de variables. n <= m - * x Vecteur de taille "n" contenant en entree une estimation - * initiale de la solution. - * ldfjac Taille dominante de la matrice "fjac". ldfjac >= "m". - * ftol Erreur relative desiree dans la somme des carre. La + * ENTREE : + * fcn Fonction qui calcule la fonction et le jacobien de la + *fonction. m Nombre de fonctions. + * n Nombre de variables. n <= m + * x Vecteur de taille "n" contenant en entree une estimation + * initiale de la solution. + * ldfjac Taille dominante de la matrice "fjac". ldfjac >= "m". + * ftol Erreur relative desiree dans la somme des carre. La *terminaison survient quand les preductions estimee et vraie de la somme des - * carres sont toutes deux au moins egal a ftol. - * xtol Erreur relative desiree dans la solution approximee. La - * terminaison survient quand l'erreur relative entre deux - * iterations consecutives est au moins egal a xtol. - * gtol Mesure de l'orthogonalite entre le vecteur des fonctions et + * carres sont toutes deux au moins egal a ftol. + * xtol Erreur relative desiree dans la solution approximee. La + * terminaison survient quand l'erreur relative entre deux + * iterations consecutives est au moins egal a xtol. + * gtol Mesure de l'orthogonalite entre le vecteur des fonctions et *les colonnes du jacobien. La terminaison survient quand le cosinus de *l'angle entre fvec et n'importe quelle colonne du jacobien est au moins - *egal a gtol, en valeur absolue. maxfev Nombre d'appel maximum. La + *egal a gtol, en valeur absolue. maxfev Nombre d'appel maximum. La *terminaison se produit lorsque le nombre d'appel a fcn avec iflag = 1 a *atteint "maxfev". - * diag Vecteur de taille "n". Si mode = 1 (voir ci-apres), diag est - * initialisee en interne. Si mode = 2, diag doit contenir les - * entree positives qui servent de facteurs d'echelle aux + * diag Vecteur de taille "n". Si mode = 1 (voir ci-apres), diag est + * initialisee en interne. Si mode = 2, diag doit contenir les + * entree positives qui servent de facteurs d'echelle aux *variables. - * mode Si mode = 1, les variables seront mis a l'echelle en interne. - * Si mode = 2, la mise a l'echelle est specifie par l'entree + * mode Si mode = 1, les variables seront mis a l'echelle en interne. + * Si mode = 2, la mise a l'echelle est specifie par l'entree *diag. Les autres valeurs de mode sont equivalents a mode = 1. factor *Definit la limite de l'etape initial. Cette limite est initialise au *produit de "factor" et de la norme euclidienne de "diag" * "x" sinon nul. @@ -878,57 +873,57 @@ int qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doub *apres chaque nprint iteration, x, fvec, et fjac sont disponible pour *impression, cela avant de quitter la procedure. Si "nprint" est negatif, *aucun appel special de fcn est faite. wa1, wa2, wa3 Vecteur de travail de - *taille "n". wa4 Vecteur de travail de taille "m". - * - * - * SORTIE : - * x Vecteur de taille "n" contenant en sortie l'estimee finale - * de la solution. - * fvec Vecteur de taille "m" contenant les fonctions evaluee en "x". - * fjac Matrice de taille "m" x "n". La sous matrice superieure de - * taille "n" x "n" de fjac contient une matrice triangulaire - * superieure r dont les elements diagonaux, classe dans le sens - * decroissant de leur valeur, sont de la forme : - * - * T T T - * p * (jac * jac) * p = r * r - * - * Ou p est une matrice de permutation et jac est le jacobien - * final calcule. - * La colonne j de p est la colonne ipvt (j) (voir ci apres) de - * la matrice identite. La partie trapesoidale inferieure de fjac - * contient les information genere durant le calcul de r. - * info Information de l'execution de la procedure. Lorsque la + *taille "n". wa4 Vecteur de travail de taille "m". + * + * + * SORTIE : + * x Vecteur de taille "n" contenant en sortie l'estimee finale + * de la solution. + * fvec Vecteur de taille "m" contenant les fonctions evaluee en "x". + * fjac Matrice de taille "m" x "n". La sous matrice superieure de + * taille "n" x "n" de fjac contient une matrice triangulaire + * superieure r dont les elements diagonaux, classe dans le sens + * decroissant de leur valeur, sont de la forme : + * + * T T T + * p * (jac * jac) * p = r * r + * + * Ou p est une matrice de permutation et jac est le jacobien + * final calcule. + * La colonne j de p est la colonne ipvt (j) (voir ci apres) de + * la matrice identite. La partie trapesoidale inferieure de fjac + * contient les information genere durant le calcul de r. + * info Information de l'execution de la procedure. Lorsque la *procedure a termine son execution, "info" est inialisee a la valeur - * (negative) de iflag. sinon elle prend les valeurs suivantes : - * info = 0 : parametres en entree non valides. - * info = 1 : les reductions relatives reelle et estimee de la - * somme des carres sont au moins egales a ftol. - * info = 2 : erreur relative entre deux iteres consecutives sont - * egaux a xtol. - * info = 3 : conditions info = 1 et info = 2 tous deux requis. - * info = 4 : le cosinus de l'angle entre fvec et n'importe + * (negative) de iflag. sinon elle prend les valeurs suivantes : + * info = 0 : parametres en entree non valides. + * info = 1 : les reductions relatives reelle et estimee de la + * somme des carres sont au moins egales a ftol. + * info = 2 : erreur relative entre deux iteres consecutives sont + * egaux a xtol. + * info = 3 : conditions info = 1 et info = 2 tous deux requis. + * info = 4 : le cosinus de l'angle entre fvec et n'importe *quelle colonne du jacobien est au moins egal a gtol, en valeur absolue. info *= 5 : nombre d'appels a fcn avec iflag = 1 a atteint maxfev. info = 6 : *ftol est trop petit. Plus moyen de reduire de la somme des carres. info = *7 : xtol est trop petit. Plus d'amelioration possible pour approximer la *solution x. info = 8 : gtol est trop petit. "fvec" est orthogonal aux - * colonnes du jacobien a la precision machine pres. - * nfev Nombre d'appel a "fcn" avec iflag = 1. - * njev Nombre d'appel a "fcn" avec iflag = 2. - * ipvt Vecteur de taille "n". Il definit une matrice de permutation p - * tel que jac * p = q * p, ou jac est le jacbien final calcule, - * q est orthogonal (non socke) et r est triangulaire superieur, - * avec les elements diagonaux classes en ordre decroissant de - * leur valeur. La colonne j de p est ipvt[j] de la matrice - *identite. qtf Vecteur de taille n contenant les n premiers elements + * colonnes du jacobien a la precision machine pres. + * nfev Nombre d'appel a "fcn" avec iflag = 1. + * njev Nombre d'appel a "fcn" avec iflag = 2. + * ipvt Vecteur de taille "n". Il definit une matrice de permutation p + * tel que jac * p = q * p, ou jac est le jacbien final calcule, + * q est orthogonal (non socke) et r est triangulaire superieur, + * avec les elements diagonaux classes en ordre decroissant de + * leur valeur. La colonne j de p est ipvt[j] de la matrice + *identite. qtf Vecteur de taille n contenant les n premiers elements *du vecteur qT * fvec. * * DESCRIPTION : * La procedure minimize la somme de carre de m equation non lineaire a n * variables par une modification de l'algorithme de Levenberg - Marquardt. * - * REMARQUE : + * REMARQUE : * L'utilisateur doit fournir une procedure "fcn" qui calcule la fonction et * le jacobien. * "fcn" doit etre declare dans une instruction externe a la procedure et doit @@ -937,11 +932,11 @@ int qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doub **iflag) * * si iflag = 1 calcul de la fonction en x et retour de ce vecteur dans fvec. - * fjac n'est pas modifie. + * fjac n'est pas modifie. * si iflag = 2 calcul du jacobien en x et retour de cette matrice dans fjac. - * fvec n'est pas modifie. + * fvec n'est pas modifie. * - * RETOUR : + * RETOUR : * En cas de succes, la valeur zero est retournee. * Sinon la valeur -1 est retournee. */ @@ -960,7 +955,7 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, double par, pnorm, prered; double sum, temp, temp1, temp2, xnorm = 0.0; - /* epsmch est la precision machine. */ + /* epsmch est la precision machine. */ epsmch = std::numeric_limits::epsilon(); *info = 0; @@ -968,7 +963,7 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, *nfev = 0; *njev = 0; - /* verification des parametres d'entree. */ + /* verification des parametres d'entree. */ /*if (n <= 0) return 0;*/ @@ -1022,8 +1017,8 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, } /* - * evaluation de la fonction au point de depart - * et calcul de sa norme. + * evaluation de la fonction au point de depart + * et calcul de sa norme. */ iflag = 1; @@ -1049,20 +1044,20 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, fnorm = enorm(fvec, m); /* - * initialisation du parametre de Levenberg-Marquardt - * et du conteur d'iteration. + * initialisation du parametre de Levenberg-Marquardt + * et du conteur d'iteration. */ par = 0.0; iter = 1; /* - * debut de la boucle la plus externe. + * debut de la boucle la plus externe. */ while (count < (int)maxfev) { count++; /* - * calcul de la matrice jacobienne. + * calcul de la matrice jacobienne. */ iflag = 2; @@ -1087,7 +1082,7 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, } /* - * si demandee, appel de fcn pour impression des iterees. + * si demandee, appel de fcn pour impression des iterees. */ if (nprint > 0) { iflag = 0; @@ -1116,8 +1111,8 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, qrfac(n, m, fjac, ldfjac, &oncol, ipvt, n, wa1, wa2, wa3); /* - * a la premiere iteration et si mode est 1, mise a l'echelle - * en accord avec les normes des colonnes du jacobien initial. + * a la premiere iteration et si mode est 1, mise a l'echelle + * en accord avec les normes des colonnes du jacobien initial. */ if (iter == 1) { @@ -1131,9 +1126,9 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, } /* - * a la premiere iteration, calcul de la norme de x mis - * a l'echelle et initialisation de la limite delta de - * l'etape. + * a la premiere iteration, calcul de la norme de x mis + * a l'echelle et initialisation de la limite delta de + * l'etape. */ for (j = 0; j < n; j++) @@ -1148,8 +1143,8 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, } /* - * formation de (q transpose) * fvec et stockage des n premiers - * composants dans qtf. + * formation de (q transpose) * fvec et stockage des n premiers + * composants dans qtf. */ for (i = 0; i < m; i++) wa4[i] = fvec[i]; @@ -1174,7 +1169,7 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, } /* - * calcul de la norme du gradient mis a l'echelle. + * calcul de la norme du gradient mis a l'echelle. */ double gnorm = 0.0; @@ -1195,7 +1190,7 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, } /* - * test pour la convergence de la norme du gradient . + * test pour la convergence de la norme du gradient . */ if (gnorm <= gtol) @@ -1226,18 +1221,18 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, } /* - * debut de la boucle la plus interne. + * debut de la boucle la plus interne. */ ratio = 0.0; while (ratio < tol0001) { /* - * determination du parametre de Levenberg-Marquardt. + * 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. + * stockage de la direction p et x + p. calcul de la norme de p. */ for (j = 0; j < n; j++) { @@ -1249,15 +1244,15 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, pnorm = enorm(wa3, n); /* - * a la premiere iteration, ajustement de la premiere limite de - * l'etape. + * 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. + * evaluation de la fonction en x + p et calcul de leur norme. */ iflag = 1; @@ -1281,7 +1276,7 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, fnorm1 = enorm(wa4, m); /* - * calcul de la reduction reelle mise a l'echelle. + * calcul de la reduction reelle mise a l'echelle. */ actred = -1.0; @@ -1290,8 +1285,8 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, 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. + * calcul de la reduction predite mise a l'echelle et + * de la derivee directionnelle mise a l'echelle. */ for (i = 0; i < n; i++) { @@ -1308,7 +1303,7 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, dirder = -((temp1 * temp1) + (temp2 * temp2)); /* - * calcul du rapport entre la reduction reel et predit. + * calcul du rapport entre la reduction reel et predit. */ ratio = 0.0; @@ -1342,12 +1337,12 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, } /* - * test pour une iteration reussie. + * test pour une iteration reussie. */ if (ratio >= tol0001) { /* - * iteration reussie. mise a jour de x, de fvec, et de - * leurs normes. + * iteration reussie. mise a jour de x, de fvec, et de + * leurs normes. */ for (j = 0; j < n; j++) { @@ -1364,7 +1359,7 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, } /* - * tests pour convergence. + * tests pour convergence. */ if ((std::fabs(actred) <= ftol) && (prered <= ftol) && (tol5 * ratio <= 1.0)) @@ -1391,8 +1386,8 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, return (count); } /* - * tests pour termination et - * verification des tolerances. + * tests pour termination et + * verification des tolerances. */ if (*nfev >= maxfev) @@ -1421,7 +1416,7 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, return (count); } - } /* fin while ratio >=tol0001 */ + } /* fin while ratio >=tol0001 */ } /*fin while 1*/ return 0; @@ -1430,58 +1425,58 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, /* * PROCEDURE : lmder1 * - * ENTREE : - * fcn Fonction qui calcule la fonction et le jacobien de la - *fonction. m Nombre de fonctions. n Nombre de variables - *(parametres). n <= m x Vecteur de taille "n" contenant en + * ENTREE : + * fcn Fonction qui calcule la fonction et le jacobien de la + *fonction. m Nombre de fonctions. n Nombre de variables + *(parametres). n <= m x Vecteur de taille "n" contenant en *entree une estimation initiale de la solution. - * ldfjac Taille maximale de la matrice "fjac". ldfjac >= "m". - * tol Tolerance. La terminaison de la procedure survient quand - * l'algorithme estime que l'erreur relative dans la somme des - * carres est au moins egal a tol ou bien que l'erreur relative - * entre x et la solution est au moins egal atol. - * wa Vecteur de travail de taille "lwa". - * lwa Taille du vecteur "wa". wa >= 5 * n + m. - * - * - * SORTIE : - * x Vecteur de taille "n" contenant en sortie l'estimee finale - * de la solution. - * fvec Vecteur de taille "m" contenant les fonctions evaluee en "x". - * fjac Matrice de taille "m" x "n". La sous matrice superieure de - * taille "n" x "n" de fjac contient une matrice triangulaire - * superieure r dont les elements diagonaux, classe dans le sens - * decroissant de leur valeur, sont de la forme : - * - * T T T - * p * (jac * jac) * p = r * r - * - * Ou p est une matrice de permutation et jac est le jacobien - * final calcule. - * La colonne j de p est la colonne ipvt (j) (voir ci apres) de - * la matrice identite. La partie trapesoidale inferieure de fjac - * contient les information genere durant le calcul de r. - * info Information de l'executionde la procedure. Lorsque la + * ldfjac Taille maximale de la matrice "fjac". ldfjac >= "m". + * tol Tolerance. La terminaison de la procedure survient quand + * l'algorithme estime que l'erreur relative dans la somme des + * carres est au moins egal a tol ou bien que l'erreur relative + * entre x et la solution est au moins egal atol. + * wa Vecteur de travail de taille "lwa". + * lwa Taille du vecteur "wa". wa >= 5 * n + m. + * + * + * SORTIE : + * x Vecteur de taille "n" contenant en sortie l'estimee finale + * de la solution. + * fvec Vecteur de taille "m" contenant les fonctions evaluee en "x". + * fjac Matrice de taille "m" x "n". La sous matrice superieure de + * taille "n" x "n" de fjac contient une matrice triangulaire + * superieure r dont les elements diagonaux, classe dans le sens + * decroissant de leur valeur, sont de la forme : + * + * T T T + * p * (jac * jac) * p = r * r + * + * Ou p est une matrice de permutation et jac est le jacobien + * final calcule. + * La colonne j de p est la colonne ipvt (j) (voir ci apres) de + * la matrice identite. La partie trapesoidale inferieure de fjac + * contient les information genere durant le calcul de r. + * info Information de l'executionde la procedure. Lorsque la *procedure a termine son execution, "info" est inialisee a la valeur - * (negative) de iflag. sinon elle prend les valeurs suivantes : - * info = 0 : parametres en entre non valides. - * info = 1 : estimation par l'algorithme que l'erreur relative - * de la somme des carre est egal a tol. - * info = 2 : estimation par l'algorithme que l'erreur relative - * entre x et la solution est egal a tol. - * info = 3 : conditions info = 1 et info = 2 tous deux requis. - * info = 4 : fvec est orthogonal aux colonnes du jacobien. - * info = 5 : nombre d'appels a fcn avec iflag = 1 a atteint - * 100 * (n + 1). - * info = 6 : tol est trop petit. Plus moyen de reduire de la - * somme des carres. - * info = 7 : tol est trop petit. Plus d'amelioration possible - * d' approximer la solution x. - * ipvt Vecteur de taille "n". Il definit une matrice de permutation p - * tel que jac * p = q * p, ou jac est le jacbien final calcule, - * q est orthogonal (non socke) et r est triangulaire superieur, - * avec les elements diagonaux classes en ordre decroissant de - * leur valeur. La colonne j de p est ipvt[j] de la matrice + * (negative) de iflag. sinon elle prend les valeurs suivantes : + * info = 0 : parametres en entre non valides. + * info = 1 : estimation par l'algorithme que l'erreur relative + * de la somme des carre est egal a tol. + * info = 2 : estimation par l'algorithme que l'erreur relative + * entre x et la solution est egal a tol. + * info = 3 : conditions info = 1 et info = 2 tous deux requis. + * info = 4 : fvec est orthogonal aux colonnes du jacobien. + * info = 5 : nombre d'appels a fcn avec iflag = 1 a atteint + * 100 * (n + 1). + * info = 6 : tol est trop petit. Plus moyen de reduire de la + * somme des carres. + * info = 7 : tol est trop petit. Plus d'amelioration possible + * d' approximer la solution x. + * ipvt Vecteur de taille "n". Il definit une matrice de permutation p + * tel que jac * p = q * p, ou jac est le jacbien final calcule, + * q est orthogonal (non socke) et r est triangulaire superieur, + * avec les elements diagonaux classes en ordre decroissant de + * leur valeur. La colonne j de p est ipvt[j] de la matrice *identite. * * DESCRIPTION : @@ -1489,7 +1484,7 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, * variables par une modification de l'algorithme de Levenberg - Marquardt. * Cette procedure appele la procedure generale au moindre carre lmder. * - * REMARQUE : + * REMARQUE : * L'utilisateur doit fournir une procedure "fcn" qui calcule la fonction et * le jacobien. * "fcn" doit etre declare dans une instruction externe a la procedure et doit @@ -1498,11 +1493,11 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, **iflag) * * si iflag = 1 calcul de la fonction en x et retour de ce vecteur dans fvec. - * fjac n'est pas modifie. + * fjac n'est pas modifie. * si iflag = 2 calcul du jacobien en x et retour de cette matrice dans fjac. - * fvec n'est pas modifie. + * fvec n'est pas modifie. * - * RETOUR : + * RETOUR : * En cas de succes, la valeur zero est retournee. * Sinon, la valeur -1. * @@ -1524,7 +1519,7 @@ int lmder1(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, return (-1); } - /* appel a lmder */ + /* appel a lmder */ maxfev = (unsigned int)(100 * (n + 1)); ftol = tol; diff --git a/modules/vision/src/pose-estimation/vpPose.cpp b/modules/vision/src/pose-estimation/vpPose.cpp index cdd2baefd6..df7d0cfa43 100644 --- a/modules/vision/src/pose-estimation/vpPose.cpp +++ b/modules/vision/src/pose-estimation/vpPose.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: * Pose computation. - * -*****************************************************************************/ + */ /*! \file vpPose.cpp @@ -326,7 +324,6 @@ bool vpPose::coplanar(int &coplanar_plane_type, double *p_a, double *p_b, double if (fabs(dist) > distanceToPlaneForCoplanarityTest) { vpDEBUG_TRACE(10, " points are not coplanar "); - // TRACE(" points are not coplanar ") ; return false; } } @@ -580,7 +577,7 @@ bool vpPose::computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool * * @param cMo the pose of the object with regard to the camera. * @return true the pose computation was succesful. - * @return false an error occured during the pose computation. + * @return false an error occurred during the pose computation. */ bool vpPose::computePoseDementhonLagrangeVVS(vpHomogeneousMatrix &cMo) { diff --git a/modules/vision/src/pose-estimation/vpPoseDementhon.cpp b/modules/vision/src/pose-estimation/vpPoseDementhon.cpp index c2c10b1efe..f42cc3eba4 100644 --- a/modules/vision/src/pose-estimation/vpPoseDementhon.cpp +++ b/modules/vision/src/pose-estimation/vpPoseDementhon.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Pose computation. - * - * Authors: - * Francois Chaumette - * -*****************************************************************************/ + */ #include #include diff --git a/modules/vision/src/pose-estimation/vpPoseFeatures.cpp b/modules/vision/src/pose-estimation/vpPoseFeatures.cpp index e3148d3851..119c62e32c 100644 --- a/modules/vision/src/pose-estimation/vpPoseFeatures.cpp +++ b/modules/vision/src/pose-estimation/vpPoseFeatures.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Pose computation from any features. - * - * Authors: - * Aurelien Yol - * -*****************************************************************************/ + */ #include #ifdef VISP_HAVE_MODULE_VISUAL_FEATURES diff --git a/modules/vision/src/pose-estimation/vpPoseLagrange.cpp b/modules/vision/src/pose-estimation/vpPoseLagrange.cpp index 0c3d316e25..a16192bd13 100644 --- a/modules/vision/src/pose-estimation/vpPoseLagrange.cpp +++ b/modules/vision/src/pose-estimation/vpPoseLagrange.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Pose computation. - * - * Authors: - * Francois Chaumette - * -*****************************************************************************/ + */ #include @@ -42,10 +37,10 @@ #define DEBUG_LEVEL2 0 /**********************************************************************/ -/* FONCTION : CalculTranslation */ -/* ROLE : Calcul de la translation entre la */ +/* FONCTION : CalculTranslation */ +/* ROLE : Calcul de la translation entre la */ /* camera et l'outil connaissant la */ -/* rotation */ +/* rotation */ /**********************************************************************/ static void calculTranslation(vpMatrix &a, vpMatrix &b, unsigned int nl, unsigned int nc1, unsigned int nc3, @@ -72,8 +67,8 @@ static void calculTranslation(vpMatrix &a, vpMatrix &b, unsigned int nl, unsigne vpMatrix cta; vpMatrix ctb; - cta = ct * a; /* C^T A */ - ctb = ct * b; /* C^T B */ + cta = ct * a; /* C^T A */ + ctb = ct * b; /* C^T B */ #if (DEBUG_LEVEL2) { @@ -122,8 +117,8 @@ static void calculTranslation(vpMatrix &a, vpMatrix &b, unsigned int nl, unsigne // FONCTION LAGRANGE : // ------------------- // Resolution d'un systeme lineaire de la forme A x1 + B x2 = 0 -// sous la contrainte || x1 || = 1 -// ou A est de dimension nl x nc1 et B nl x nc2 +// sous la contrainte || x1 || = 1 +// ou A est de dimension nl x nc1 et B nl x nc2 //********************************************************************* //#define EPS 1.e-5 @@ -204,7 +199,7 @@ static void lagrange(vpMatrix &a, vpMatrix &b, vpColVector &x1, vpColVector &x2) // vpMatrix v ; e.svd(x1, ata); // destructif sur e // calcul du vecteur propre de E correspondant a la valeur propre min. - /* calcul de SVmax */ + /* calcul de SVmax */ imin = 0; // FC : Pourquoi calculer SVmax ?????? // double svm = 0.0; @@ -212,7 +207,7 @@ static void lagrange(vpMatrix &a, vpMatrix &b, vpColVector &x1, vpColVector &x2) // { // if (x1[i] > svm) { svm = x1[i]; imin = i; } // } - // svm *= EPS; /* pour le rang */ + // svm *= EPS; /* pour le rang */ for (i = 0; i < x1.getRows(); i++) if (x1[i] < x1[imin]) @@ -404,7 +399,7 @@ void vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, bool *p_isPlan, double * } #endif - if (X2[5] < 0.0) { /* to obtain Zo > 0 */ + if (X2[5] < 0.0) { /* to obtain Zo > 0 */ for (unsigned int i = 0; i < 3; i++) X1[i] = -X1[i]; for (unsigned int i = 0; i < 6; i++) @@ -416,7 +411,7 @@ void vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, bool *p_isPlan, double * } for (unsigned int i = 0; i < 3; i++) { X2[i] -= (s * X1[i]); - } /* X1^T X2 = 0 */ + } /* X1^T X2 = 0 */ // s = 0.0; // for (i=0;i<3;i++) {s += (X2[i]*X2[i]);} @@ -439,7 +434,7 @@ void vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, bool *p_isPlan, double * s = 1.0 / sqrt(s); for (unsigned int i = 0; i < 3; i++) { X2[i] *= s; - } /* X2^T X2 = 1 */ + } /* X2^T X2 = 1 */ calculTranslation(A, B, nl, 3, 3, X1, X2); @@ -454,7 +449,7 @@ void vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, bool *p_isPlan, double * 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]); - /* calcul de la matrice de passage */ + /* calcul de la matrice de passage */ for (unsigned int i = 0; i < 3; i++) { cMf[i][0] = X1[i]; cMf[i][1] = X2[i]; @@ -556,7 +551,7 @@ void vpPose::poseLagrangeNonPlan(vpHomogeneousMatrix &cMo) } #endif - if (X2[8] < 0.0) { /* car Zo > 0 */ + if (X2[8] < 0.0) { /* car Zo > 0 */ X1 *= -1; X2 *= -1; } @@ -566,7 +561,7 @@ void vpPose::poseLagrangeNonPlan(vpHomogeneousMatrix &cMo) } for (i = 0; i < 3; i++) { X2[i] -= (s * X1[i]); - } /* X1^T X2 = 0 */ + } /* X1^T X2 = 0 */ // s = 0.0; // for (i=0;i<3;i++) {s += (X2[i]*X2[i]);} @@ -590,7 +585,7 @@ void vpPose::poseLagrangeNonPlan(vpHomogeneousMatrix &cMo) s = 1.0 / sqrt(s); for (i = 0; i < 3; i++) { X2[i] *= s; - } /* X2^T X2 = 1 */ + } /* X2^T X2 = 1 */ X2[3] = (X1[1] * X2[2]) - (X1[2] * X2[1]); X2[4] = (X1[2] * X2[0]) - (X1[0] * X2[2]); diff --git a/modules/vision/src/pose-estimation/vpPoseLowe.cpp b/modules/vision/src/pose-estimation/vpPoseLowe.cpp index 116ab3b8d2..3f83a26453 100644 --- a/modules/vision/src/pose-estimation/vpPoseLowe.cpp +++ b/modules/vision/src/pose-estimation/vpPoseLowe.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Pose computation. - * - * Authors: - * Francois Chaumette - * -*****************************************************************************/ + */ #include #include // numeric_limits @@ -42,7 +37,7 @@ #include // besoin de la librairie mathematique, en particulier des -// fonctions de minimisation de Levenberg Marquartd +// fonctions de minimization de Levenberg Marquartd #include #include @@ -59,22 +54,22 @@ // ------------------------------------------------------------------------ /* - * MACRO : MIJ + * MACRO : MIJ * - * ENTREE : - * m Matrice. - * i Indice ligne de l'element. - * j Indice colonne de l'element. - * s Taille en nombre d'elements d'une ligne de la matrice "m". + * ENTREE : + * m Matrice. + * i Indice ligne de l'element. + * j Indice colonne de l'element. + * s Taille en nombre d'elements d'une ligne de la matrice "m". * - * DESCRIPTION : + * DESCRIPTION : * La macro-instruction calcule l'adresse de l'element de la "i"eme ligne et * de la "j"eme colonne de la matrice "m", soit &m[i][j]. * - * RETOUR : + * RETOUR : * L'adresse de m[i][j] est retournee. * - * HISTORIQUE : + * HISTORIQUE : * 1.00 - 11/02/93 - Original. */ #define MIJ(m, i, j, s) ((m) + ((long)(i) * (long)(s)) + (long)(j)) @@ -112,25 +107,25 @@ void eval_function(int npt, double *xc, double *f) } /* - * PROCEDURE : fcn + * PROCEDURE : fcn * - * ENTREES : - * m Nombre d'equations. - * n Nombre de variables. - * xc Valeur courante des parametres. - * fvecc Resultat de l'evaluation de la fonction. - * ldfjac Plus grande dimension de la matrice jac. - * iflag Choix du calcul de la fonction ou du jacobien. + * ENTREES : + * m Nombre d'equations. + * n Nombre de variables. + * xc Valeur courante des parametres. + * fvecc Resultat de l'evaluation de la fonction. + * ldfjac Plus grande dimension de la matrice jac. + * iflag Choix du calcul de la fonction ou du jacobien. * - * SORTIE : - * jac Jacobien de la fonction. + * SORTIE : + * jac Jacobien de la fonction. * - * DESCRIPTION : + * DESCRIPTION : * La procedure calcule la fonction et le jacobien. * Si iflag == 1, la procedure calcule la fonction en "xc" et le resultat est - * stocke dans "fvecc" et "fjac" reste inchange. + * stocke dans "fvecc" et "fjac" reste inchange. * Si iflag == 2, la procedure calcule le jacobien en "xc" et le resultat est - * stocke dans "fjac" et "fvecc" reste inchange. + * stocke dans "fjac" et "fvecc" reste inchange. * * HISTORIQUE : * 1.00 - xx/xx/xx - Original. @@ -172,10 +167,10 @@ void fcn(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int i for (int i = 0; i < npt; i++) { double x = XO[i]; - double y = YO[i]; /* coordonnees du point i */ + double y = YO[i]; /* coordonnees du point i */ double z = ZO[i]; - /* coordonnees du point i dans le repere camera */ + /* 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]; @@ -247,7 +242,7 @@ void fcn(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int i *MIJ(jac, 5, npt + i, ldfjac) = 0.0; } } - } /* fin else if iflag ==2 */ + } /* fin else if iflag ==2 */ } /*! @@ -270,14 +265,14 @@ void vpPose::poseLowe(vpHomogeneousMatrix &cMo) int tst_lmder; double f[2 * NBPTMAX], sol[NBR_PAR]; double tol, jac[NBR_PAR][2 * NBPTMAX], wa[2 * NBPTMAX + 50]; - // double u[3]; /* vecteur de rotation */ - // double rd[3][3]; /* matrice de rotation */ + // double u[3]; /* vecteur de rotation */ + // double rd[3][3]; /* matrice de rotation */ - n = NBR_PAR; /* nombres d'inconnues */ - m = (int)(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 */ + n = NBR_PAR; /* nombres d'inconnues */ + m = (int)(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 */ // c = cam ; // for (i=0;i<3;i++) @@ -305,7 +300,7 @@ void vpPose::poseLowe(vpHomogeneousMatrix &cMo) tst_lmder = lmder1(&fcn, m, n, sol, f, &jac[0][0], ldfjac, tol, &info, ipvt, lwa, wa); if (tst_lmder == -1) { std::cout << " in CCalculPose::PoseLowe(...) : "; - std::cout << "pb de minimisation, returns FATAL_ERROR"; + std::cout << "pb de minimization, returns FATAL_ERROR"; // return FATAL_ERROR ; } diff --git a/modules/vision/src/pose-estimation/vpPoseRGBD.cpp b/modules/vision/src/pose-estimation/vpPoseRGBD.cpp index 4c7d516f0d..59d082dcc0 100644 --- a/modules/vision/src/pose-estimation/vpPoseRGBD.cpp +++ b/modules/vision/src/pose-estimation/vpPoseRGBD.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: * Pose computation from RGBD. - * -*****************************************************************************/ + */ #include #include diff --git a/modules/vision/src/pose-estimation/vpPoseRansac.cpp b/modules/vision/src/pose-estimation/vpPoseRansac.cpp index 05f09d8670..9c9d6528d5 100644 --- a/modules/vision/src/pose-estimation/vpPoseRansac.cpp +++ b/modules/vision/src/pose-estimation/vpPoseRansac.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: * Pose computation. - * -*****************************************************************************/ + */ /*! \file vpPoseRansac.cpp diff --git a/modules/vision/src/pose-estimation/vpPoseVirtualVisualServoing.cpp b/modules/vision/src/pose-estimation/vpPoseVirtualVisualServoing.cpp index 791e5dc9bc..7c7e0bfbfc 100644 --- a/modules/vision/src/pose-estimation/vpPoseVirtualVisualServoing.cpp +++ b/modules/vision/src/pose-estimation/vpPoseVirtualVisualServoing.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: * Pose computation. - * -*****************************************************************************/ + */ /*! \file vpPoseVirtualVisualServoing.cpp diff --git a/modules/vision/test/homography/testDisplacement.cpp b/modules/vision/test/homography/testDisplacement.cpp index fb7e2feccb..961d2033e6 100644 --- a/modules/vision/test/homography/testDisplacement.cpp +++ b/modules/vision/test/homography/testDisplacement.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: * Tests transformation within various representations of rotation. - * -*****************************************************************************/ + */ /*! \file testDisplacement.cpp diff --git a/modules/vision/test/keypoint-with-dataset/testKeyPoint-2.cpp b/modules/vision/test/keypoint-with-dataset/testKeyPoint-2.cpp index 16dcc40539..4234bd2f76 100644 --- a/modules/vision/test/keypoint-with-dataset/testKeyPoint-2.cpp +++ b/modules/vision/test/keypoint-with-dataset/testKeyPoint-2.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 keypoint matching and pose estimation. - * -*****************************************************************************/ + */ #include diff --git a/modules/vision/test/keypoint-with-dataset/testKeyPoint-3.cpp b/modules/vision/test/keypoint-with-dataset/testKeyPoint-3.cpp index ec432dfda7..1587b3c40f 100644 --- a/modules/vision/test/keypoint-with-dataset/testKeyPoint-3.cpp +++ b/modules/vision/test/keypoint-with-dataset/testKeyPoint-3.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -31,8 +30,7 @@ * Description: * Test keypoint matching with mostly OpenCV functions calls * to detect potential memory leaks in testKeyPoint.cpp. - * -*****************************************************************************/ + */ #include diff --git a/modules/vision/test/keypoint-with-dataset/testKeyPoint-4.cpp b/modules/vision/test/keypoint-with-dataset/testKeyPoint-4.cpp index a94f312b01..012ed3ed94 100644 --- a/modules/vision/test/keypoint-with-dataset/testKeyPoint-4.cpp +++ b/modules/vision/test/keypoint-with-dataset/testKeyPoint-4.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -31,8 +30,7 @@ * Description: * Test keypoint matching and pose estimation with mostly OpenCV functions * calls to detect potential memory leaks in testKeyPoint-2.cpp. - * -*****************************************************************************/ + */ #include diff --git a/modules/vision/test/keypoint-with-dataset/testKeyPoint-5.cpp b/modules/vision/test/keypoint-with-dataset/testKeyPoint-5.cpp index ed9256803d..ebe751d8c2 100644 --- a/modules/vision/test/keypoint-with-dataset/testKeyPoint-5.cpp +++ b/modules/vision/test/keypoint-with-dataset/testKeyPoint-5.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -31,8 +30,7 @@ * Description: * Test keypoints detection with OpenCV, specially the Pyramid implementation * feature misssing in OpenCV 3.0. - * -*****************************************************************************/ + */ #include diff --git a/modules/vision/test/keypoint-with-dataset/testKeyPoint-6.cpp b/modules/vision/test/keypoint-with-dataset/testKeyPoint-6.cpp index 7a471a2edd..3fe0ee74cb 100644 --- a/modules/vision/test/keypoint-with-dataset/testKeyPoint-6.cpp +++ b/modules/vision/test/keypoint-with-dataset/testKeyPoint-6.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 descriptor computation. - * -*****************************************************************************/ + */ #include diff --git a/modules/vision/test/keypoint-with-dataset/testKeyPoint-7.cpp b/modules/vision/test/keypoint-with-dataset/testKeyPoint-7.cpp index 108d318d81..611e1f5b15 100644 --- a/modules/vision/test/keypoint-with-dataset/testKeyPoint-7.cpp +++ b/modules/vision/test/keypoint-with-dataset/testKeyPoint-7.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 saving / loading learning files for vpKeyPoint class. - * -*****************************************************************************/ + */ #include #include diff --git a/modules/vision/test/keypoint-with-dataset/testKeyPoint.cpp b/modules/vision/test/keypoint-with-dataset/testKeyPoint.cpp index c2e698a568..3771d0c945 100644 --- a/modules/vision/test/keypoint-with-dataset/testKeyPoint.cpp +++ b/modules/vision/test/keypoint-with-dataset/testKeyPoint.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 keypoint matching. - * -*****************************************************************************/ + */ #include diff --git a/modules/vision/test/keypoint-with-dataset/testXmlConfigParserKeyPoint.cpp b/modules/vision/test/keypoint-with-dataset/testXmlConfigParserKeyPoint.cpp index fe8e65f9eb..9f5116eae4 100644 --- a/modules/vision/test/keypoint-with-dataset/testXmlConfigParserKeyPoint.cpp +++ b/modules/vision/test/keypoint-with-dataset/testXmlConfigParserKeyPoint.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 vpXmlConfigParserKeyPoint parse / save. - * -*****************************************************************************/ + */ /*! \file testXmlConfigParserKeyPoint.cpp diff --git a/modules/vision/test/pose-with-dataset/testPoseRansac2.cpp b/modules/vision/test/pose-with-dataset/testPoseRansac2.cpp index ab430a89b2..19a6cb3ac0 100644 --- a/modules/vision/test/pose-with-dataset/testPoseRansac2.cpp +++ b/modules/vision/test/pose-with-dataset/testPoseRansac2.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 RANSAC 3D pose estimation method. - * -*****************************************************************************/ + */ #include diff --git a/modules/vision/test/pose/testFindMatch.cpp b/modules/vision/test/pose/testFindMatch.cpp index 3e0f3961b9..4dbd410b7e 100644 --- a/modules/vision/test/pose/testFindMatch.cpp +++ b/modules/vision/test/pose/testFindMatch.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -32,11 +31,7 @@ * Compute the pose of a 3D object using the Dementhon method. Assuming that * the correspondance between 2D points and 3D points is not done, we use * the RANSAC algorithm to achieve this task - * - * Authors: - * Aurelien Yol - * -*****************************************************************************/ + */ #include #include diff --git a/modules/vision/test/pose/testPose.cpp b/modules/vision/test/pose/testPose.cpp index 8ddc42a8f9..c09a94c99b 100644 --- a/modules/vision/test/pose/testPose.cpp +++ b/modules/vision/test/pose/testPose.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -31,8 +30,7 @@ * Description: * Compute the pose of a 3D object using the Dementhon, Lagrange and * Non-Linear approach. - * -*****************************************************************************/ + */ #include #include diff --git a/modules/vision/test/pose/testPoseFeatures.cpp b/modules/vision/test/pose/testPoseFeatures.cpp index f13d293ad8..56d6e32548 100644 --- a/modules/vision/test/pose/testPoseFeatures.cpp +++ b/modules/vision/test/pose/testPoseFeatures.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,11 +29,7 @@ * * Description: * Compute the pose from visual features by virtual visual servoing. - * - * Authors: - * Aurelien Yol - * -*****************************************************************************/ + */ #include #include diff --git a/modules/vision/test/pose/testPoseRansac.cpp b/modules/vision/test/pose/testPoseRansac.cpp index da3271f10a..8c7f8213c0 100644 --- a/modules/vision/test/pose/testPoseRansac.cpp +++ b/modules/vision/test/pose/testPoseRansac.cpp @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -32,11 +31,7 @@ * Compute the pose of a 3D object using the Dementhon method. Assuming that * the correspondance between 2D points and 3D points is not done, we use * the RANSAC algorithm to achieve this task - * - * Authors: - * Aurelien Yol - * -*****************************************************************************/ + */ #include #include diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureException.h b/modules/visual_features/include/visp3/visual_features/vpFeatureException.h index b188973a9f..cd76aa0eca 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureException.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureException.h @@ -1,5 +1,4 @@ -/**************************************************************************** - * +/* * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2023 by Inria. All rights reserved. * @@ -30,58 +29,50 @@ * * Description: * Exception that can be emitted by the vpFeature class and its derivatives. - * -*****************************************************************************/ + */ #ifndef _vpFeatureException_h_ #define _vpFeatureException_h_ -/* ------------------------------------------------------------------------- - */ -/* --- INCLUDE ------------------------------------------------------------- - */ -/* ------------------------------------------------------------------------- - */ - -/* \file vpFeatureException.h - \brief error that can be emitted by the vpFeature class and its derivatives +/*! + * \file vpFeatureException.h + * \brief error that can be emitted by the vpFeature class and its derivatives */ -/* Classes standards. */ -#include /* Classe std::ostream. */ -#include /* Classe string. */ +#include +#include #include #include -/* ------------------------------------------------------------------------- - */ -/* --- CLASS --------------------------------------------------------------- - */ -/* ------------------------------------------------------------------------- - */ - /*! - \class vpFeatureException - \ingroup group_visual_features - \brief Error that can be emitted by the vpBasicFeature class and its - derivates. + * \class vpFeatureException + * \ingroup group_visual_features + * \brief Error that can be emitted by the vpBasicFeature class and its + * derivates. */ class VISP_EXPORT vpFeatureException : public vpException { public: /*! - \brief Lists the possible error than can be emitted while calling - vpFeature member - */ - enum errorFeatureCodeEnum { - //! feature list or desired feature list is empty + * \brief Lists the possible error than can be emitted while calling + * vpFeature member + */ + enum errorFeatureCodeEnum + { +//! Feature list or desired feature list is empty badErrorVectorError, + //! Size mismatch error sizeMismatchError, + //! Feature not initialized notInitializedError, + //! Wrong feature initialization badInitializationError }; public: + /*! + * Constructor. + */ vpFeatureException(int id, const char *format, ...) { this->code = id; @@ -90,7 +81,15 @@ class VISP_EXPORT vpFeatureException : public vpException setMessage(format, args); va_end(args); } + + /*! + * Constructor. + */ vpFeatureException(int id, const std::string &msg) : vpException(id, msg) { } + + /*! + * Constructor. + */ explicit vpFeatureException(int id) : vpException(id) { } }; diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCommon.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCommon.h index 5e38df964c..b5047577fc 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCommon.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCommon.h @@ -217,7 +217,7 @@ Interaction Matrix Ls 4.353086256e-17 -1.339411156e-16 -0 -0.03019436997 -0.0168230563 -1 Error vector (s-s*) -0.1 0 0 1.831867991e-15 -1.072059108e-15 0 -Gain : Zero= 1 Inf= 1 Deriv= 0 +Gain : Zero= 1 Inf= 1 Deriv= 0 \endcode */ diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentDatabase.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentDatabase.h index 3de86b69db..6ee0450bc1 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentDatabase.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentDatabase.h @@ -157,11 +157,11 @@ int main() class VISP_EXPORT vpFeatureMomentDatabase { private: - struct cmp_str { + struct vpCmpStr_t { bool operator()(const char *a, const char *b) const { return std::strcmp(a, b) < 0; } char *operator=(const char *) { return NULL; } // Only to avoid a warning under Visual with /Wall flag }; - std::map featureMomentsDataBase; + std::map featureMomentsDataBase; void add(vpFeatureMoment &featureMoment, char *name); public: diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentGravityCenter.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentGravityCenter.h index c5c68256b9..2a5cea3d14 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentGravityCenter.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentGravityCenter.h @@ -99,7 +99,7 @@ int main() //only contain the basic moment. vpMomentBasic bm; //basic moment (this particular moment is nothing //more than a shortcut to the vpMomentObject) - vpMomentGravityCenter gc; //gravity center + vpMomentGravityCenter gc; //gravity center bm.linkTo(mdb); //add basic moment to moment database gc.linkTo(mdb); //add gravity center to moment database diff --git a/modules/visual_features/include/visp3/visual_features/vpFeaturePointPolar.h b/modules/visual_features/include/visp3/visual_features/vpFeaturePointPolar.h index b0c24894a7..6e69fb9aa8 100644 --- a/modules/visual_features/include/visp3/visual_features/vpFeaturePointPolar.h +++ b/modules/visual_features/include/visp3/visual_features/vpFeaturePointPolar.h @@ -85,7 +85,7 @@ \begin{array}{cccccc} \frac{-\cos \theta}{Z} & \frac{-\sin \theta}{Z} & \frac{\rho}{Z} & (1+\rho^2)\sin\theta & -(1+\rho^2)\cos\theta & 0 \\ - \;\\ \ + \;\\ \ \frac{\sin\theta}{\rho Z} & \frac{-\cos\theta}{\rho Z} & 0 & \cos\theta /\rho & \sin\theta/\rho & -1 \\ \end{array} \right] \f] diff --git a/modules/visual_features/src/feature-builder/vpFeatureBuilderLine.cpp b/modules/visual_features/src/feature-builder/vpFeatureBuilderLine.cpp index 5816ea4ba8..606ccc8311 100644 --- a/modules/visual_features/src/feature-builder/vpFeatureBuilderLine.cpp +++ b/modules/visual_features/src/feature-builder/vpFeatureBuilderLine.cpp @@ -221,30 +221,8 @@ void vpFeatureBuilder::create(vpFeatureLine &s, const vpCameraParameters &cam, c while (theta < -M_PI) { theta += 2 * M_PI; } - // vpTRACE("meter %f %f",rho, theta) ; - /* - while(theta < -M_PI) theta += 2*M_PI ; - while(theta >= M_PI) theta -= 2*M_PI ; - - // If theta is between -90 and -180 get the equivalent - // between 0 and 90 - if(theta <-M_PI/2) - { - theta += M_PI ; - rho *= -1 ; - } - // If theta is between 90 and 180 get the equivalent - // between 0 and -90 - if(theta >M_PI/2) - { - theta -= M_PI ; - rho *= -1 ; - } - */ s.buildFrom(rho, theta); - // vpTRACE("meter %f %f",rho, theta) ; - } catch (...) { vpERROR_TRACE("Error caught"); throw; diff --git a/modules/visual_features/src/visual-feature/vpBasicFeature.cpp b/modules/visual_features/src/visual-feature/vpBasicFeature.cpp index 4ae7c68b41..cb3b1a6be3 100644 --- a/modules/visual_features/src/visual-feature/vpBasicFeature.cpp +++ b/modules/visual_features/src/visual-feature/vpBasicFeature.cpp @@ -102,7 +102,6 @@ unsigned int vpBasicFeature::getDimension(unsigned int select) const if (dim_s > 31) return dim_s; for (unsigned int i = 0; i < s.getRows(); i++) { - // printf("%x %x %d \n",select, featureLine[i], featureLine[i] & select); if (FEATURE_LINE[i] & select) dim += 1; } diff --git a/modules/visual_features/src/visual-feature/vpFeatureMomentDatabase.cpp b/modules/visual_features/src/visual-feature/vpFeatureMomentDatabase.cpp index 94a3603787..be6c1a5653 100644 --- a/modules/visual_features/src/visual-feature/vpFeatureMomentDatabase.cpp +++ b/modules/visual_features/src/visual-feature/vpFeatureMomentDatabase.cpp @@ -63,8 +63,8 @@ void vpFeatureMomentDatabase::add(vpFeatureMoment &featureMoment, char *name) */ vpFeatureMoment &vpFeatureMomentDatabase::get(const char *type, bool &found) { - std::map::const_iterator it = - featureMomentsDataBase.find(type); + std::map::const_iterator it = + featureMomentsDataBase.find(type); found = (it != featureMomentsDataBase.end()); return *(it->second); @@ -79,7 +79,7 @@ vpFeatureMoment &vpFeatureMomentDatabase::get(const char *type, bool &found) */ void vpFeatureMomentDatabase::updateAll(double A, double B, double C) { - std::map::const_iterator itr; + std::map::const_iterator itr; #ifdef VISP_HAVE_OPENMP std::vector values; values.reserve(featureMomentsDataBase.size()); @@ -101,7 +101,7 @@ void vpFeatureMomentDatabase::updateAll(double A, double B, double C) /* std::ostream & operator<<(std::ostream & os, const vpFeatureMomentDatabase& m){ std::map::const_iterator itr; os << +char*,vpMoment*,vpFeatureMomentDatabase::vpCmpStr_t>::const_iterator itr; os << "{"; for(itr = m.featureMoments.begin(); itr != m.featureMoments.end(); itr++){ diff --git a/modules/vs/include/visp3/vs/vpServoException.h b/modules/vs/include/visp3/vs/vpServoException.h index 4aceab9ce6..2615f2ab26 100644 --- a/modules/vs/include/visp3/vs/vpServoException.h +++ b/modules/vs/include/visp3/vs/vpServoException.h @@ -37,37 +37,29 @@ #define _vpServoException_h_ /*! - \file vpServoException.h - \brief error that can be emitted by the vpServo class and its derivatives -*/ + * \file vpServoException.h + * \brief error that can be emitted by the vpServo class and its derivatives + */ #include -#include /* Classe std::ostream. */ -#include /* Classe string. */ - -/* ------------------------------------------------------------------------- - */ -/* --- CLASS --------------------------------------------------------------- - */ -/* ------------------------------------------------------------------------- - */ +#include +#include /*! - \class vpServoException - \brief Error that can be emitted by the vpServo class and its derivatives. - \author Eric Marchand (Eric.Marchand@irisa.fr) Irisa / Inria Rennes + * \class vpServoException + * \brief Error that can be emitted by the vpServo class and its derivatives. */ class VISP_EXPORT vpServoException : public vpException { public: /*! - - \brief Lists the possible error than can be emitted while calling - vpServo member - */ - enum errorServoCodeEnum { - //! Current or desired feature list is empty + * \brief Lists the possible error than can be emitted while calling + * vpServo member + */ + enum errorServoCodeEnum + { +//! Current or desired feature list is empty noFeatureError, //! No degree of freedom is available to achieve the secondary task. noDofFree, @@ -78,6 +70,9 @@ class VISP_EXPORT vpServoException : public vpException }; public: + /*! + * Constructor. + */ vpServoException(int id, const char *format, ...) { this->code = id; @@ -86,7 +81,15 @@ class VISP_EXPORT vpServoException : public vpException setMessage(format, args); va_end(args); } + + /*! + * Constructor. + */ vpServoException(int id, const std::string &msg) : vpException(id, msg) { } + + /*! + * Constructor. + */ explicit vpServoException(int id) : vpException(id) { } }; diff --git a/platforms/android/service/engine/src/org/visp/engine/ViSPEngineService.java b/platforms/android/service/engine/src/org/visp/engine/ViSPEngineService.java index 525be6bee4..f98565b7e6 100644 --- a/platforms/android/service/engine/src/org/visp/engine/ViSPEngineService.java +++ b/platforms/android/service/engine/src/org/visp/engine/ViSPEngineService.java @@ -88,7 +88,7 @@ public String getFileList() { public void onCreate() { Log.d(TAG, "Service starting"); - for (Field field : R.xml.class.getDeclaredFields()) { // Build error here means that all config.xml files are missing (configuration problem) + for (Field field : R.xml.class.getDeclaredFields()) { // Build error here means that all config.xml files are missing (configuration problem) Log.d(TAG, "Found config: " + field.getName()); final LibVariant lib = new LibVariant(); try { diff --git a/platforms/scripts/valgrind/valgrind_visp.supp b/platforms/scripts/valgrind/valgrind_visp.supp index fbefa45ff2..2388a189bf 100644 --- a/platforms/scripts/valgrind/valgrind_visp.supp +++ b/platforms/scripts/valgrind/valgrind_visp.supp @@ -1,6 +1,6 @@ ################################################################### # -# Error that occured on flylab-gs3 ci running ubuntu 18.04 +# Error that occurred on flylab-gs3 ci running ubuntu 18.04 # ################################################################### @@ -21,7 +21,7 @@ ################################################################### # -# Error that occured on ViSP-ubuntu-12.04-i386 ci running ubuntu 12.04 +# Error that occurred on ViSP-ubuntu-12.04-i386 ci running ubuntu 12.04 # ################################################################### @@ -221,7 +221,7 @@ ################################################################### # -# Error that occured on visp-centos-7-2-amd64 ci +# Error that occurred on visp-centos-7-2-amd64 ci # ################################################################### @@ -362,7 +362,7 @@ ################################################################### # -# Errors that occured on visp-osx-10-9-mavericks ci +# Errors that occurred on visp-osx-10-9-mavericks ci # ################################################################### @@ -686,4 +686,3 @@ fun:currentlocale fun:_ZN21vpMbtXmlGenericParserC2Ei } - diff --git a/platforms/scripts/valgrind/valgrind_visp_3rdparty.supp b/platforms/scripts/valgrind/valgrind_visp_3rdparty.supp index 1b359f624b..4f3d340c7e 100644 --- a/platforms/scripts/valgrind/valgrind_visp_3rdparty.supp +++ b/platforms/scripts/valgrind/valgrind_visp_3rdparty.supp @@ -1,7 +1,7 @@ ################################################################### # -# Errors that occured on visp-osx-10-9-mavericks ci +# Errors that occurred on visp-osx-10-9-mavericks ci # ################################################################### @@ -27,7 +27,7 @@ ################################################################### # -# Errors that occured on flylab-gs3 running ubuntu 18.04 +# Errors that occurred on flylab-gs3 running ubuntu 18.04 # ################################################################### @@ -157,4 +157,3 @@ ... fun:dlopen@@GLIBC_2.2.5 } - diff --git a/tutorial/detection/dnn/tutorial-dnn-object-detection-live.cpp b/tutorial/detection/dnn/tutorial-dnn-object-detection-live.cpp index cbe8721f40..a52634f795 100644 --- a/tutorial/detection/dnn/tutorial-dnn-object-detection-live.cpp +++ b/tutorial/detection/dnn/tutorial-dnn-object-detection-live.cpp @@ -12,7 +12,7 @@ #ifdef VISP_HAVE_NLOHMANN_JSON #include -using json = nlohmann::json; +using json = nlohmann::json; //! json namespace shortcut #endif typedef enum diff --git a/tutorial/java/tag-detection/ApriltagDetection.java b/tutorial/java/tag-detection/ApriltagDetection.java index 91c41b9392..152b1cb1e2 100644 --- a/tutorial/java/tag-detection/ApriltagDetection.java +++ b/tutorial/java/tag-detection/ApriltagDetection.java @@ -72,7 +72,7 @@ public class ApriltagDetection extends JFrame { private static String[] poseEstimationMethodNames = {"HOMOGRAPHY", "HOMOGRAPHY_VIRTUAL_VS", "DEMENTHON_VIRTUAL_VS", "LAGRANGE_VIRTUAL_VS", "BEST_RESIDUAL_VIRTUAL_VS", "HOMOGRAPHY_ORTHOGONAL_ITERATION"}; @SuppressWarnings("deprecation") - private Object[][] data = { { Integer.valueOf(-1), Double.valueOf(0.053) } }; + private Object[][] data = { { Integer.valueOf(-1), Double.valueOf(0.053) } }; private JTextArea poseArea; public ApriltagDetection() { @@ -613,4 +613,3 @@ public void run() { }); } } - diff --git a/tutorial/matlab/tutorial-matlab.cpp b/tutorial/matlab/tutorial-matlab.cpp index add36dd2b3..f365026e6e 100644 --- a/tutorial/matlab/tutorial-matlab.cpp +++ b/tutorial/matlab/tutorial-matlab.cpp @@ -1,7 +1,7 @@ -/*! \example tutorial-matlab.cpp */ -/* - * Tutorial using ViSP and MATLAB - * Determine column-wise sum of ViSP matrix using MATLAB Engine +/*! \example tutorial-matlab.cpp + * + * Tutorial using ViSP and MATLAB + * Determine column-wise sum of ViSP matrix using MATLAB Engine */ #include #include diff --git a/tutorial/simulator/image/target_square.fig b/tutorial/simulator/image/target_square.fig index b1aecdabda..718cebb797 100644 --- a/tutorial/simulator/image/target_square.fig +++ b/tutorial/simulator/image/target_square.fig @@ -2,7 +2,7 @@ Landscape Center Metric -Letter +Letter 100.00 Single -2 @@ -12,4 +12,4 @@ Single 1 3 0 1 0 0 50 -1 20 0.000 1 0.0000 7200 7200 675 675 7200 7200 7875 7200 1 3 0 1 0 0 50 -1 20 0.000 1 0.0000 2700 7200 675 675 2700 7200 3375 7200 2 2 0 10 0 7 50 -1 -1 0.000 0 0 -1 0 0 5 - 450 450 9450 450 9450 9450 450 9450 450 450 + 450 450 9450 450 9450 9450 450 9450 450 450 diff --git a/tutorial/tracking/dnn/tutorial-megapose-live-single-object-tracking.cpp b/tutorial/tracking/dnn/tutorial-megapose-live-single-object-tracking.cpp index d6144c30b6..e418b1eb3c 100644 --- a/tutorial/tracking/dnn/tutorial-megapose-live-single-object-tracking.cpp +++ b/tutorial/tracking/dnn/tutorial-megapose-live-single-object-tracking.cpp @@ -21,7 +21,7 @@ #include -using json = nlohmann::json; +using json = nlohmann::json; //! json namespace shortcut /* * Interpolate two vpColors. Linear interpolation between each components (R, G, B) diff --git a/tutorial/tracking/model-based/generic-rgbd/tutorial-mb-generic-tracker-rgbd-realsense-json-settings.cpp b/tutorial/tracking/model-based/generic-rgbd/tutorial-mb-generic-tracker-rgbd-realsense-json-settings.cpp index f2334ce983..9813864be5 100644 --- a/tutorial/tracking/model-based/generic-rgbd/tutorial-mb-generic-tracker-rgbd-realsense-json-settings.cpp +++ b/tutorial/tracking/model-based/generic-rgbd/tutorial-mb-generic-tracker-rgbd-realsense-json-settings.cpp @@ -13,7 +13,7 @@ #include #include -using json = nlohmann::json; +using json = nlohmann::json; //! json namespace shortcut int main(int argc, char *argv []) diff --git a/tutorial/visual-servo/ibvs/target_square.fig b/tutorial/visual-servo/ibvs/target_square.fig index b1aecdabda..718cebb797 100644 --- a/tutorial/visual-servo/ibvs/target_square.fig +++ b/tutorial/visual-servo/ibvs/target_square.fig @@ -2,7 +2,7 @@ Landscape Center Metric -Letter +Letter 100.00 Single -2 @@ -12,4 +12,4 @@ Single 1 3 0 1 0 0 50 -1 20 0.000 1 0.0000 7200 7200 675 675 7200 7200 7875 7200 1 3 0 1 0 0 50 -1 20 0.000 1 0.0000 2700 7200 675 675 2700 7200 3375 7200 2 2 0 10 0 7 50 -1 -1 0.000 0 0 -1 0 0 5 - 450 450 9450 450 9450 9450 450 9450 450 450 + 450 450 9450 450 9450 9450 450 9450 450 450 diff --git a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-json.cpp b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-json.cpp index 991657b049..406f860664 100644 --- a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-json.cpp +++ b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-json.cpp @@ -9,7 +9,7 @@ #include -using json = nlohmann::json; +using json = nlohmann::json; //! json namespace shortcut //! [Enum] enum vpInteractionMatrixTypeSubset